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Chapter 1 
INTRODUCTION 


1.1. General Background . 

It is obvious that in nature animals use quite a different 
method of locomotion over land than do vehicles designed by man. 
Animals, including man, use independent limbs to walk over 
solid surfaces, but man-made vehicles almost exclusively use 
wheels. Each of these approaches has its advantages. Wheeled 
vehicles on hard surfaces allow very fast and efficient move- 
ment of heavy loads. Legs, on the other hand, usually move more 
slowly but do not require hard, smooth surfaces. 

Although wheeled vehicles have been in use for centuries, 
only relatively recently has much research been conducted on 
the use of legs for man-made vehicles. This is due to the fact 
that the coordination of limb motions to nroduce locomotion of 
a legged vehicle is a very complex process. Advances in the 
speed and capabilities of digital computers have finally allowed 
this burden to be removed from a human contioller, making arti- 
ficial legged locomotion a practicable goal. Vehicles such as 
the Mars Rover [1] which use legged vehicle concepts are now 
being considered for use in terrain too rough for wheeled 
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vehicles. 


1.2. Objectives of this Work. 

It has also been observed that multi-armed vehicles with 
grasping hands could walk over structures even in the absence 
of gravity [2] . Since serious consideration is being given to 
the construction of large structures in space [3], the problem 
of coordinating the limb motion of a vehicle which could perform 
assembly and maintenance operations on those structures is of 
practical as well as theoretical interest. That problem is the 
subject of this work. 

1.3. Organization . 

The remainder of this work is divided into five parts. 
Chapters 2 through 6. Chapter 2 reviews earlier research which 
is relevant to this work. This includes the previous work on 
manipulator kinematics and that on walking robots. 

Chapter 3 is a statement of the problem which this work 
addresses. The robot itself is described and some introductory 
material is presented on the coordinate systems used. The 
basic control scheme which is implemented is also discussed in 
this chapter. 

Chapter 4 addresses the problem of the control of the 
individual arms. Arm velocities are generally described in 
Cartesian coordinates, so the first section of the chapter 


discusses the conversion from Cartesian velocli Les to joint 
velocities using the Jacobian matrix. The second section of 
the chapter describes the calculation of a trajectory for an 
arm given a sequence of points through which it is to pass. 

Chapter 5 is a discussion of the free gai . algorithm 
which controls the lifting and placing of legs for the robot. 
The first section describes the generation of commanded velo- 
cities for the robot, and the second section discusses the 
implementation of those velocities by the algorithm. 

Chapter 6 summarizes the results of the simulation. It 
also gives suggestions ^~r further work in the area of robot 
legged locomotion. 
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Chapter 2 

SURVEY OF PREVIOUS WORK 

2 • 1 . Introduction . 

The coordination of the limbs of a walking robot which 
uses manipulators as its legs (arms) can be decomposed into 
two tasks. The first task is the control of the individual 
manipulators’ movements, and the second is the coordination of 
the manipulators to produce walking. Previous work in both 
these areas is reviewed in this chapter. 

2.2. Previous Work on Manipulator Kinematics. 

2.2.1. Cart esian-Coordinate- to-Joint-Coordinate Conversion 

In the control of a manipulator, it is desirable to be able 
to give commands to the manipulator in Cartesian coordinates, 
since that is the system in which hvmans are most accustomed to 
working. The manipulator’s motors operate in joint coordinates, 
though, so a method must be found for converting from one system 
to the other. There are two basic approaches to this problem: 
physical and mathematical. 

The physical approach uses the manipulator itself or a 
replica of it to record motion directly in joint coordinates. 


A copy of the manipulator is used in master-slave control, in 
which the replic" often smaller, of the arm is moved by a human 
operator. The lepiica has potentiometers on the joints whose 
values are used to obtain commands for the manipulator’s motors. 
Another physical approach, sometimes referred to as ’’training," 
involves moving the manipulator itself tnrough its desired tra- 
jectory, measuring and recording intermediate positions al^ng 
the way in joint coordinates. These data, along with timing 
information, constitute a command sequence. This method is 
often applied when a manipulator is used in a situation such 
as an assembly line in which the same sequence of motions is 
repeated over and over [4] . 

Neither of the above physical approaches is satisfactory 
for legged locomotion. Master-slave control does not work well 
because a human being cannot effectively control the number of 
legs required for locomotion except at very slow speeds [5]. 
Training is not applicable since the trajectory which a leg must 
follow will vary as the terrain and/or the operator’s commands 
change . 

Thus for legged locomotion a mathematical approach is 
needed to convert the commands from Cartesian coordinates to 
joint coordinates. Although this could be done for commands 
given as positions and orientations, most manipulators have 
several sets of joint coordinates which will result in the 
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same end effector position and orientation [6]. A better 
approach is that of Resolved Motion Rate Control [7], in which 
velocity commands are used. For instance, with a six-degree-of 
freedom arm, if £ is a vector consisting of all six joint velo- 
cities and X is a vector consisting of the translational velo- 
cities along and the rotational velocities about the x, y, and 
z axes, 

X = J 0_ , (2-1) 


VJl.sre J is the Jacobian matrix of derivatives of the Cartesian 
coordinate values (x) with respect to the joint angles (£) : 

3x, 


■^ij " 3 9, 


(2-2) 


Joint velocities can then be found from the rectilinear velo- 
cities using the inverse Jacobian matrix: 


9 = 



X 


(2-3) 


The advantage of using velocities rather than positions 
is that for a six-degree-of-freedom arm, there is one and only 
one set of joint velocities that will produce a given trans^ 
lational and rotational velocity of the end effector, except in 
those configurations in which the Jacobian matrix is singular. 
At a singularity of the Jacobian, velocity in at least one 
direction is impossible, and velocities in other directions may 
be obtainable with an infinite number of combinations of joint 
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velocities. Some work has been done on the general problem of 
finding which configurations will result in Jacobian singulari- 
ties for a particular arm [8]. For anns with more than six 
degrees of i.reedom, there are generally an infinite number of 
ways to attain a given velocity, and the extra degrees of free- 
dom can be used to optimize some criterion (e.g., to minimize 
the sum of the joint velocities) [9, 10]. 

Another possible approach to the problem of determining 
the joint velocities is that of full or partial table look-up. 
Alt bough in this work only kinematics are considered, in an 
actual implementation the torques required to produce a set of 
joint velocities would also have to be determined, and it is 
possible that the two problems could be combined. If that could 
be done, one of the published methods for torque solution by 
table look-up could be used. Possible approaches would be the 
full table look-up of Albus’s ^Cerebellar Model Articulation 
Controller,'^ or CMAC [11], or the partial look-ups suggested by 
Horn and Raibert [12, 13]. 

2.2.2. Homogeneous Transformation Matrices - 

It will be seen later that it is desirable to be able to 
express the position and orientation of one part of the robot 
with respect to another. A compact way of doing this which will 
be used in this work is the homogeneous transforaation matrix, 
which was introduced by Denavit and Hartenberg [14] and has 
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been applied more specifically to manipulators by Pieper [15] 
and Paul [16]. 

A homogeneous transformation matrix is a 4x4 matrix of 
the form shown in Equation (2-4) • 

(2-4) 

It describes the position and orientation of one coordinate 
system (say (x\ , z.’)) with respect to another (x, ^) . 

Orientation information is carried in the upper left 3x3 sub- 
matrix, R, which is a rotation matrix whose columns are the 
unit vectors , and expressed in (x, ^) coordinates. 

Position information is in the upper right 3x1 submatrix, £, 
which is a vector that gives the position of the origin of 
(x ’ , , £* ) in (5, z_) coordinates. The last row consists 

of three zeros and a one. 

As mentioT .d above, the 3x3 rotation submatrix of the 
homogeneous transformation matrix contains relative orientation 
information for coordinate systems (x’ , _y ’ , ’ ) and (S, £) . 

Another method of expressing the orientation relationship be- 
tween two coordinate systems which will also be used in this 
work is that of the three Euler angles a» T shown in 

Figure 1. The transformation of system (x ’ , £* , z_’ ) into 
system (x, _z) is accomplished by a rotation of -y about , 


0 0 0 1 
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Figure 1. Definition of Euler Angles a, g, y Which Specify 
Orientation of Coordinate System (x' , , z') 

with respect to Coordinate System (2, 


a rotation of -6 about , and a rotation of -ot about £_* , in 
that order. The rotation matrix R can then be expressed in 
••.erms of these Euler angles as [6] 


R' 


cosacosgeosy-sinasiny -^osacosgsiny-sinacosy cosasing 
sinacosgcosy+cosasiny -sinacosgsiny+cosacosy sinasing 
-sinBcosy singsiny cosg 


(2 


Then, if is a direction vector in system , the 

same direction vector £ in system (£, £) is 


( 2 - 6 ) 


If the position of the origin of (x* * X* > 1 .' ^ coordi- 
nate system (S, 2^) is £* , the homogeneous transformation 

matrix A describing the position and orientation of system 
, ^') with respect to system (x, 2.> i) then 


[R] 


(2-7) 


Then if (r^, r^, r^ 

(B.' > 1 * B') 


are the coordinates of a point in system 

r , r ) are the coordinates of that point 
y z 


in system (x, _z) 



( 2 - 8 ) 


2.3. Previous Work on Walking Robots. 

In the past two decades, research has begun on walking 
robots in hope that their advantages can eventually be used for 
efficient transportation over terrain unsuitable for wheeled 
vehicles. A summary of this research has been presented by 
McGhee [17], and the next section presents those portions of 
that summary which are relevant to the present work. The final 
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section of this chapter discusses the problems of gait selection 
and foot placement for a walking robot. 

2.3.1. Legged Vehicle Control. 

Three different approaches have been considered for the 
control of legged locomotion systems. These approaches are 
finite-state control, model reference control, and algorithmic 
control, each of which is discussed in the following paragraphs. 

In 1961, Tomovic [18] noted that some modes of locomotion 
could be characterized by finite-state models. He observed that 
locomotion could be characterized by representing each leg as 
a two-state device (on the ground or in the air) . This concept 
was extended when Tomovic and McGhee [19] pointed out that at 
least four actuator states are needed to obtain a more or less 
natural gait. The validity of the finite-state control concept 
was confirmed in 1968 when a small quadruped controlled by 16 
flip-flops attained stable locomotion at the University of 
Southern California [20] . 

A second approach to control of legged systems is "model 
reference control [21]." This approach uses a real-time simu- 
lation of vehicle kinematics for the generation of the joint 
angles and rates needed for ideal locomotion. These ideal 
values are then used as commands to the actuators of the vehicle. 
This method has the advantage of being much more flexible than 
finite-state control, since the actuator control signals are 
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infinitely variable instead of having a finite number of states, 
but the problem of deciding when the vehicle should lift its 
legs and when and where it should place them must be solved 
before it can be used. This problem is discussed in the next 
section. 

A variant of model reference control has been suggested 
by Vukobratovic [22]. With this approach, called algorithmic 
control, a nominal trajectory is obtained from a kinematic model 
as with model reference control. With algorithmic control, 
though, not all joint angles and rates are derived from the 
model. Instead, the model imposes certain overall constraints 
on the system to lower the order of its equations. These 
lower-order equations are then used with artificial sensor 
feedback to produce stable motion. To date, this technique has 
been applied only to biped locomotion. 

2.3.2. Gait Selection and Foot Placement. 

A considerable amount of past work has been devoted to the 
leg sequencing problem for the special case of straight-line, 
constant-speed locomotion over level terrain. A periodic 
solution to this problem has usually been referred to as a 
"gait.” Combinatorial studies show that there are a large num- 
ber of gaits possible (40,000,000 for a six-legged vehicle [23]), 
so optimization is required. In all but one study [24], the opti- 
mization criterion used has been one related to the degree of 
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stability of the system under consideration. In most of the 
studies the stability criterion used has been the longitudinal 
stability margin, the distance the vehicle's center of gravity 
would have to be moved forward or backward before the vehicle 
toppled over. For vehicles whose legs are evenly spaced in 
right-left pairs along a longitudinal motion axis, the gaits 
which maximize the longitudinal stability margin are known [25] 
Those gaits are known as wave gaits , all of which are character 
ized by a forward wave of stepping actions on each side of the 
body with a half-cycle phase shift between the members of each 
right-left pair of legs. 

More recently, Kugushev and Jaroshevskij [24] have sug- 
gested that the approach used in the study of gaits for 
straight-line locomotion can be extended to include a more 
general case in which nonperiodic leg sequences known as "free 
gaits" may be expected. Specifically, they present a partial 
problem formulation in which a trajectory is specified in ad- 
vance for the center of gravity of a legged system over a given 
terrain containing certain regions which are unsuitable for 
support. The remainder of the paper is devoted to a completion 
of the formalization of this problem and a description of one 
heuristic algorithm for its solution. 

McGhee and Iswandhi [26] have presented a more complete 
formalization of the free gait problem- Central to their 
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approach, which is basically similar to that of Kugushev and 
Jaroshevskij , is the concept of kinematic margin. The kinematic 
margin for a particular leg and foothold is the distance for 
which the vehicle can continue along its present path before 
that leg must be lifted from that foothold. McGhee and Iswandhi*s 
algorithm proceeds by lifting and placing legs so as to maximize 
the minimum value of kinematic margin over all supporting legs. 
This approach maximizes the distance the vehicle can proceed 
forward in its current configuration, thereby increasing the 
likelihood that it will be able to find a new configuration with 
which to continue. 

2.4. Summary. 

This chapter has summarized some of the research in the 
general areas of manipulator kinematics and walking robots. 
Chapter 3 will present the specific problem to be addressed in 
this work, and Chapters 4 and 5 will discuss the solution of 
that problem. 
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Chapter 3 

PROBLEM FORMULATION 


3.1. Introduction . 

As was mentioned in the first chapter, there are situa- 
tions in which a mobile robot with six-degree-of-freedom arms 
capable of grasping handholds would be very useful. It is the 
purpose of this work to describe an algorithm for controlling 
the motion of such a vehicle. In this chapter, the design of 
the robot which was used in the simulation is presented, and 
the overall control philosophy is explained. 

The first section of the chapter is divided into two 
parts: The first describes the robot’s body and the second 

its arms. The second section of the chapter presents the 
control scheme that is to be implemented. The basic approach 
chosen is that of supervisory control [27], in which a human 
operator provides only high-level commands, such as robot 
velocity or destination. A control computer then automatically 
solves the limb coordination problem to move the vehicle in 
response to the operator’s commands. 


3.2, Robot Description, 

The robot investigated here consists of a body and three 
six-degree-of-freedora arms [28]. Descriptions of the body and 
ams are given in the following paragraphs. 

3.2.1. Body Model. 

Figure 2 is a drawing of the three-armed vehicle. The 
body is modelled as an equilateral triangle with an arm attached 
to each corner. The next few paragraphs discuss this model and 
the coordinate systems used to describe it. 

For calculations of "absolute” positions and velocities, 
both of the robot and of its arras, a coordinate system which 
is fixed relative to the surface on which the robot walks is 
required. For this purpose a right-handed coordinate system 
(x, z_) (circumflexes will indicate unit vectors) is defined. 

Since this system is fixed relative to the surface on which the 
robot is walking, its definition depends on the robot’s in- 
tended use. If the robot is to be used on the earth, the 
system would be an earth-fixed coordinate system; if it is to 
be used in space, it would be a system fixed to the structure 
on which it is walking. In this work, the vehicle is assumed 
to be operating on a cylinder in space, so the coordinate 
system is chosen to be fixed relative to that cylinder (see 
Figure 2) . 

Also shown in Figure 2 is a coordinate system, 

» which is fixed to the moving vehicle. Throughout 
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this work, subscripts will be used to indicate the coordinate 
system to which a unit vector belongs; the absence of a sub- 
script will indicate the fixed coordinate system. Thus is 
the unit vector in the x direction for the vehicle coordinate 
system, and x belongs to the fixed system. 

Vectors will be used to describe the position or velocity 
of the robot's body or one of its arms with respect to the 
cylinder, the body, or another arm. There will be a coordinate 
system fixed to each part of the robot (including the body and 
each link of each arm) as well as the cylinder, so these vectors 
can more conveniently be thought of as describing the position 
or velocity of one coordinate system with respect to another. 
Subscripts will be used to indicate the system whose location 
or motion is being referred to, and superscripts will denote 
the coordinate system to which that location or motion is 
referred (again, absence of a superscript will indicate the 
fixed system) . Thus the position and linear velocity of the 
origin of the vehicle coordinate system x^ith respect to the 
fixed system will be denoted by and y^, respectively. The 
angular velocity of the vehicle system with respect to the 
fixed system will be denoted by a three-element vector 
whose elements are the angular rotation rates about and 

The orientation of the vehicle system with respect to the 
fixed system can most concisely be described by three Euler 
angles, a^, and There is no standard convention for 
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choosing these angles; the ones used in this work are those 
described by Horn and Inoue [6] and shown in Figure 1 (in 
Chapter 2) . 

A vector in vehicle coordinates can be transformed into 
fixed coordinates by multiplying by a 3x3 rotation matrix, ^ 
(in this work, matrices will be denoted by underlined, upper- 
case xetters); that is. 


r - 



(3-1) 


where ^ is of the form given in Equation (2-4) . The coordi- 
nates of a point in vehicle coordinates can also be transformed 
to fixed coordinates by multiplying them by a matrix, in this 
case a 4x4 homogeneous transformation matrix, A ; thus 



- - 


r- T 

V 



^x 

[R J P„ 


V 

—V V 

y 

Pv 

Z 


V 

1 

o 

o 

o 

1 

1 

-1 - 


(3-2) 


To simplify the equations in this thesis, the above equation 
will be written 


r = A r 


V 


(3-3) 


If the matrix in the equation is a homogeneous transformation 

matrix, the vector _r is to be understood as a four-element 

T 

vector (r , r , r , 1) ; if the matrix is a rotation matrix, 

X y z ’ 
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as in Equation (3-1), is the three-element vector 
nT 


3.2.2, Ann Model. 

/ drawing of one of the robot’s arms is shown in Figure 3. 
The configuration and dimensions of the arm are those of a six- 
degree-of-f reedom industrial manipulator, the PUMA 600, made 
by Unimation, Inc., Danbury, Connecticut. The arm has six 
rotational joints, one about each of the z-axes except 
(numerical subscripts indicate references to link coordinate 
systems). The angular displacement of joint i will be denoted 
by 0^; 6^ is zero when joint i is at the center of its travel 
and positive when the displacement is in a positive sense with 
respect Figure 3 each of the joints is pictured 

at the center of its travel; the rotational limits of the 
joints are given in Table I. 


TABLE I 

PUMA 600 Manipulator Joint Limits 


Joint 

Limits 

1 

±160 “ 

2 

±165° 

3 

±135° 

4 

±135° 

5 

±105° 

6 

±270° 
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Figure 4. Definitions of General Link Coordinate 
Systems and Parameters Describing Re- 
lationships between Coordinate Systems 

The arm is made up of seven links (including the base) , 
with each two successive links connected by a rotary joint. 
Associated with each link is a coordinate system which is fixed 
with respect to that link (see Figure 4) . For link coordinate 
system i, is directed along the axis of the joint between 
link i and link i+1, x^ is along the common normal between the 
two joint axes associated with link i in the direction from 
toward and is chosen to complete the right-handed 
set [15]- 

Link coordinate system i can be transformed into link 
system i-1 by performing a rotation, two translations and a 
final rotation as follows (refer to Figure 4) : 

1. A rotation of -a^ about x^ to make parallel 
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2. 


A translation of -a^ along to locate the origin 
at the point where the common normal between and 

intersects 

3. A translation of -s^ along to bring the origins 
into coincidence. 

4. A rotation of about to bring into coinci- 
dence with X. (0’ may differ from 0. since this 

method of describing 0^ does not necessarily result 
in its being equal to zero at the center of joint i’s 
travel, as 0^ was defined to be.) 

Each of the above four operations can be described by a 4x4 
transformation matrix, and the product of those four matrices 
is the homogeneous transformation matrix which describes the 
overall transformation from coordinate system i to system i-1; 
that is , 


,i-l 





r 

- 

" 


rot^ ft* 

L 


trans^ 

L ^i‘-®iJ 

trans/s 

L ^i’-^J 

rot/N 

^i*-“iJ 


(3-4) 


where 


rot 

trans 


r,0 


r,s 


is a rotation of 0 about _r 
is a translation of s along r^. 


If the above multiplications are carried out, A^ ^ can also be 
expressed in a more useful form as a function of the four joint 
parameters as [14] 
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COSO : 
1 

sin0 ! 


-cosOt.sinB , 
1 i 

COSOt.COS0! 
1 1 

sinot . 

1 


sina.sin0, 
1 i 

“Sinot.cos0 ! 

X X 


a.cos9 . 

X X 

a^sinS^ 


TABLE II 

Homogeneous Transformation Matrix Parameters for 
PUMA 600 Manipulator Joints 


Joxnt, i 


0 

NO 

9. + 180“ 



X 

ES 

SN 

02 + 90“ 

0 

0 

9^ + 90“ 

0 

WE 

9, + 180“ 

0 

0 

0g + 180“ 

0 

HW 

9. 
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NO = neck-to-base distance = 26 in, 

SN =s shoulder-to-neck distance = 6 in, 
ES = elbow-shoulder distance = 17 in. 
WE = wrist-to-elbow distance = 17 in. 
HW = hand-to-wrist distance = 6 in. 


The four parameters for each joint which determine the 
transformation matrices between link systems are given in 
Table II. When those parameters are introduced into Equation 







(3-5), the following six transformation matrices result; 
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-cos6, 

-sin0^ 

0 

0 
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be specified by the three Euler angles a , (where 

ae ee ee 

these are defined as in Figure 1) , and the angular velocity 
of the system by . 

Then the orientation of the end effector system with 

respect to the arm base system can be described by the 3x3 

rotation matrix which is, again, of the form given in 
— ee 

Equation (2-4) • As with the robot system, both position and 
orientation information are contained in the homogeneous trans- 
formation matrix, in this case A , where 



(3-16) 


3.3. Control System. 

As was mentioned earlier in this chapter , the control 
system described in this work is a supervisory control system 
[27] in which the higher-level commands of steering, speed, 
etc., are supplied to the control computer by a human operator. 
The computer then calculates the arm motions which are required 
to implement those commands. 

Another method of dividing the control problem into 
levels applicable to walking robots has been suggested by 
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Narinyani, et al, [29] . This model divides the problem into 
three levels. The upper level is the overall planning of the 
route based on the robot’s basic capabilities and limitations 
(for instance, the size of obstacle it can overcome). The 
middle level involves looking ahead for several steps in the 
direction of the route fixed by the upper level. This level 
then arrives at a corridor of possible paths based on robot 
parameters (speed, balance, body height, etc.) and local 
terrain features. The lower level involves placing the feet 
to move the robot along one of the paths in the corridor 
determined by the middle level. 

In many cases, it would be useful to consider the lower 
level as consisting of two levels: One which determines the 

location at which a foot is to be placed, and another which 
calculates the trajectory which the foot should follow in 
reaching that location. This division seems justified since 
in difficult terrain human beings often consider where to place 
their feet but they rarely think about the actual path their 
feet follow in the air. This fact can be applied to walking 
robots if access is allowed to the program between the above 
two levels. Then in difficult terrain a human operator could 
determine where the robot’s feet should be placed and let the 
computer determine how to get them there. This mode of oper- 
ation would also be useful if one of the robot’s legs is to be 
as an arm (to manipulate tools, for example), a circumstance 
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which might well occur with the robot in this work, since its 
legs are industrial manipulators. 

If Narinyani's lower level is divided as suggested above, 
the functional division of robot control into levels becomes 

Level I - overall planning of route based on robot’s 
basic capabilities and limitations 
Level II - determining corridor of paths based on robot 
parameters and local terrain features 
Level III - choosing locations at which feet are placed 
Level IV - calculating foot trajectories 

Although in the general robot walking problem Level II is very 
important, in this work the terrain is structured enough that 
Levels I and II can be combinedo There are generally enough 
footholds that the robot’s flexibility allows it to move along 
any path specified for its center of gravity. 

The above four levels constitute a model of the general 
robot walking problem. For the specific problem addressed in 
this work, three modes of operation were chosen: 

1. Robot Destination Control. 

2. Robot Velocity Control. 

3. Arm Control. 

In the following paragraphs these three modes are discussed 
in terms of the four levels described above. 
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With Robot Destination Control, the operator enters a 
sequence of points (in coordinates based on the grid on the 
cylinder), and the computer calculates the trajectory required 
for the robot to pass through those points. In this mode, the 
computer performs the operations required for part of Levels I 
and II and all of Levels III and IV, 

With Robot Velocity Control, a three-axis joystick is used. 
Two of the axes control the linear velocity of the robot on the 
grid, and the other axis controls the rotation rate of the 
robot body. In this mode the operator handles Levels I and II 
and the computer performs the calculations for Levels III and 
IV. 

Arm Control can be used in three different way^. In all 
three of those, the robot is stationary, so Levels i and II 
are not operating. With Arm Handhold Control, the operator 
enters a handhold location (in grid coordinates) and the arm 
moves to and grasps that handhold. With Arm Destination Con- 
trol, a sequence of desired coordinates (positions and orien- 
tations) for the arm is entered and the computer calculates the 
trajectory required to pass through those points. In these 
two modes the computer performs only the operations for Level 
IV. Finally, with Arm Velocity Control, the translational 
velocity of the end effector is controlled by the three-axis 
joystick, and its rotational velocity is controlled by a set of 
three dials. In this mode all of the levels listed above are 
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handled by the operator, with the computer only converting 
commands from Cartesian coordinates to joint coordinates. 

3.4. Summary. 

In this chapter, some of the necessary background infor- 
mation has been given for the problem of control of the three- 
armed robot discussed in this work. Models for the body of the 
robot and for its arms have been given, and the control system 
employed has been outlined. Chapters 4 and 5 will discuss the 
solution of the problem formulated in this chapter. 


Chapter 4 

AEM KINEMATICS ALGORITHMS 


4 • 1 . Introduction . 

The problem of meinipulator kinematics can conveniently 
be divided into two parts • The first is the conversion of 
Cartesian velocity commands to joint velocity commands. As 
discussed in Chapter 2, it is usually convenient to derive 
manipulator commands in Cartesian coordinates since that is 
the system in which human beings are most accustomed to working. 
Those commands must then be converted to joint coordinates for 
the actuator signals. This problem is the topic of Section 4.2. 

Although the Cartesian velocity commands can be provided 
at every instant in tjjne by a joystick or similar device, in 
many cases the desired path of the manipulator is specified in 
the form of a sequence of points through which it is to pass. 
Deriving a trajectory (Cartesian velocities as functions of 
time) from that sequence of points is the second part of the 
manipulator kinematics problem and is discussed in Section 4.3. 
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4,2. Jacobian Solution. 

The most straightforward way of deriving joint velocities 
from Cartesian velocities is to use basic Resolved Motion Rate 
Control [7]. For a six-degree-of-freedom manipulator thj.s in- 
volves first calculating the 6x6 Jacobian matrix whose ele- 
ments are the partial derivatives with respect to the joint 

a a 

angles of the components of and ^^(the translational and 
rotational velocities of the end effector with respect to che 
arm base). That is. 


(4-1) 


Then to derive joint velocities from Cartesian velocities (v 

— ee 

and 0) ) the Jacobian must be inverted and multiplied by those 

— ee 

Cartesian velocities: 


e = j 


-1 |4e 


(4-2) 


Using this method for a six-degree-of-freedom arm involves in- 
verting a 6x6 matrix. This can be done symbolically, which is 
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very difficult, or numerically every time the velocities are 
calculated, which is computationally expensive. Since neither 
of the above alternatives is desirable, an alternative solution 
has been used in this work. For some arm designs, including 
the one used here, the joint velocity problem can be divided 
into two p'-.rts, each of which requires the inversion of a 3x3 
matrix. These two 3x3 matrices can be inverted symbolically, 
eliminating the need for numerical inversions at run time. This 
approach is discussed more fully in Section 4.2.1. 

No matter how the joint velocity problem is solved, there 
are some arm configurations (those in which the Jacobian is 
singular) in which Cartesian velocities in one or more direc- 
tions are impossible. Although mathematically these singulari- 
ties are all similar, the physical limitations they place on 
the arm may be very different sc they all must be considered 
individually. The three singularities of the PUMA 600 arm are 
considered in Section 4.2.2, 

4.2.1. Arm Decomposition Approach. 

In the PUMA 600 arm the last three joint axes intersect 
in point which can be called the "wrist.” This allows the 
first three and last three joint velocities to be calculated 
independently, given the desired translational and rotational 
velocity of the end effector. The procedure which is used to 
calculate those joint velocities is described in the following 
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paragraphs . 

The first step is to calculate the translational velocity 
of the wrist, which is located at the origin of coordinate sys- 
tem (^, • That velocity depends only on the first 

three joint velocities, so if it is known, those joint veloci- 
ties can be determined. Since the end effector and wrist are 
connected by a rigid link, the wrist's translational velocity 
(y^) can be shown to be the sum of the end effector trans- 
lational velocity (y?) and the cross product of the end effector 

o 


rotational velocity (^) and a vector from the end effector to 

— o 

the wrist Chat is, 


0 0 0 ^ , 0 0 . 

^ X (24~£^) 


( 4 - 3 ) 


Then to calculate the first three joint velocities, the 
Jacobian matrix must be found whose elements are the partial 
derivatives of the components of the wrist position, with 
respect to the first three joint angles. The easiest way to 
calculate that Jacobian is to calculate and then make the 
required partial differentiations; thus 
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and if the Jacobian is not singular the inverse Jacobian is 
(since for this arm WE = ES) 
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where = (s2+s23)s3ES^ . 


The first three joint velocities can then be calculated 


as 
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The possible singularities of (s2+s23 = 0 and s3 = 0) are 
discussed in Section 4. 2. 2.1, 

Now that the first three joint velocities are known, the 
last three joint velociites can be calculated from the end 
effector rotat ^nal velocity. This can be done by calculating 
the rotational velocity which must be provided by the last three 
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joints which is equal to the desired end effector rotation- 

al velocity ((^) minus the rotational contribution of the first 
three joints (w^) > all transformed into coordinate system (x^, 

Z3 > £3) ; "'hat is . 




(4-11) 
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Thus to calculate the last three joint velocities, the 
Jacobian matrix must be found whose elements are the partial 
derivatives of the components of the end effector orientation 
with respect to the last three joint angles. The columns of 
that Jacobian are simply the vectors describing the last three 
joint axes in coordinate system (x^, ; thus 
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and if the Jacobian is not singular the inverse Jacobian is 
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The last three joint velocities can then be calculated as 
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The possible singularity of ^2 (^^ “ 0)is discussed in Section 


4. 2. 2. 2, 


4.2.2. Jacobian Singularities. 

Both the Jacobians discussed above can be singular (have 
a determinant equal to zero). Under those conditions the joint 
velocities cannot be calculated using the inverse Jacobian since 
it does not exist. There are two ways of dealing with this 
problem: Another method can be used to calculate the joint 

velocities under those conditions or artificial limits on the 
joints can be imposed to make those conditions impossible. The 
latter approach is adopted for the singularities of and the 
former is used to the singularity of 2 ^ 2 * Those approaches are 
discussed in the following two sections. 
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0. 


4.2.2 ,1. Singularities of 

The Jacobian matrix is singular when s3 = 0 or s2+s23 = 
The condition s3 => 0 occurs when the arm is fully extended 
(i.e., when the elbow joint is straight). At this point the 
wrist cannot move farther away from the shoulder, nor can it 
move directly towards the shoulder; thus any velocity command 
which includes components in those directions will be unattain- 
able. This condition was eliminated by imposing an artificial 
joint limit of 0^ ^-1°. This limit does not allow the arm to 
be fully extended, but since it can be almost fully extended, 
the radius of the reachable space is only decreased by about 
0.01 in. The artificial limit also makes some otherwise 
reachable points near the arm base unreachable but for walking 
those points are unimportant. If one of the arms were being 
used for something other than walking (e.g., manipulating a 
tool) , the limit could be overridden and the approach outlined 
in Section 4. 2. 2. 2. could be used. 

The approach used for the singularity of s2+s23 = 0 is 
similar. At this point the wrist is directly under the shoulder 
and cannot move either toward or away from the base-neck axis. 
This condition was eliminated by imposing the limit 0^ ^ 

+ 1®. This limit increases the minimum radius for the arm by 
only 0.03 in.' It also makes an area underneath the robot un- 
reachable, but for walking that area should be avoided anyway. 
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At any rate, if it were necessary to reach that area, the limit 
could be overridden and the approach in the next section used. 


4. 2 .2. 2 Singularity of 

The Jacobian matrix singular when s5 = 0. This 

singularity cannot be handled as the two in the previous 
section were because it can occur at many positions and 
orientations in the reachable area, not just at its edges. 
Thus elimination of this singularity would place severe re- 
strictions on the motion of the arm, and a method must be 
found to solve for the joint velocities when s5 == 0. 


When 


s5 = 0, 6^ = 0"^ and the axes for 9^ and 0g are the 


same. Any change in 8^ or 0g will affect only cOg (see Equation 
(4-12)), so either 9^ or 8^ can be chosen arbitrarily so long 
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as the other is chosen so their sum is . Thus let 8^ remain 
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constant; then 
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Then to determine 9^ note that the configuration is as 

shown in Figure 5. Any change in 0^ can cause a rotation only 

is the direction of z^. Thus the only portion of the desired 

3 3 

rotational velocity (whose components are oo. and o). ) which 

Ox Oy 

can be implemented is the projection of that velocity on 
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4,3. Traj ectory Calculation . 

The problem of calculating a time-optimal trajectory for 
a robot arm to follow in passing through a sequence of points 
is an important one for this work. For reliability purposes, 
the robot will have two arms grasping handholds at all times, 
so only one arm can be moving in the air. This means that on 
the average the time it takes the robot to move one unit can be 

no less than three times the time it takes an arm in the air to 

travel the same unit with respect to the ground. An increase 
in the average velocity of the arm with respect to the ground 
will be reflected by three times that increase in the robot's 
velocity, so the degree of time optimality of the arms’ tra- 
jectories will determine how close the robot’s maximum velocity 
approaches its theoretical limit. 

Some work has been done in the area of arm trajectory 
time optimization by Luh and Walker [30]. They consider 

translational motion only in which the end effector path is 

made up of straight line segments connected by smooth arcs. The 
arcs are ’’rounding” of the comers from an exactly point-to- 
point path, and are subject to a maximum error constraint which 
is related to the length of the path segments which form the 
comers. The path is not actually a minimum-time trajectory 
because a restriction of constant* velocity is used in the 
straight segments between the connecting arcs. 
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Tr'^nslatlor.al and Rotational Velocities 


Translational Velocity Only 


Figure 6. Typical Arm Path Showing How a Point-to-Point 
Specification is Modified for a Near-Minimum- 
Time Trajectory. 

The approach used here considers both translational and 
rotational velocities. The translational path is similar to 
that of Luh and Walker in that it is composed of straight seg- 
ments connected by arcs (see Figure 6) , but the error constraint 
is absolute rather than a function of the segment lengths. 

Another difference from the previous work is that accelerations 
and decelerations are permitted in the straight segments, but 
constant acceleration is specified in the transition arcs, making 
them parabolic arcs. Since most of a path is usually straight 
segments, this approach allows the arm to travel faster in those 
segments, slowing down only to ”tum the comers.’* The algorithm 
also assumes zero rotational velocity in the transition (see 
Figure 6) , but since the desired translational displncement of 
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Figure 8. Definition of Arm Translational Trajectory Variables 

an arm is usually relatively greater than the rotational dis- 
placement, this will not often slow down the arm. A flow chart 
of the algorithm is shown in Figure 7 and it is discussed in 
detail in the following paragraphs. 

4.3.1. Translational Trajectory. 

Figure 8 shows a segment of the translational trajectory 
with some of the terms which will be used below defined. The 
figure shows segment i of the trajectory along with transitions 
i-1 and i at the ends of that segment. The angle made at the 
comer of transition i is 0^. The distance from that corner 
to the point where the actual path breaks away from an exact 
point-to-point path is d^^ . The total length of segment i is 
d^, and that length minus the two transition distances is d^. 
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and the velo- 


The velocity on entrance to transition i is ^ 

’^il 

city on exit from that transition if v . The magnitude of 

both those velocities is v . The time it takes for the arm to 

pass through transition i will be denoted by . 

i 

The first steps in the algorithm are the calculation of the 
maximum transition distances and velocities (see Figure 8) • The 
transition distance is calculated using the general formula 

As = Iv^CAt) +-|-a(At)^| (4-19) 

where As is the distance travelled, ^ is the initial velocity, 

£ is the acceleration, and At is the time interval; from that 
formula it can be calculated that 
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1 

Since a constant acceleration is specified over the entire 
transition, the time taken to make the transition can be shown 
to be the same as if the arm followed the segment paths with an 
instantaneous change of direction at the intersection. For the 
transition distance to be maximum the difference in those two 
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( 4 - 29 ) 
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If any of the segment accelerations is greater than the maximum 
acceleration allowed, the transition distances and velocities 
must be recalculated. This is done by lowering the higher of 
the two velocities to the value which requires the maximum 
acceleration. The equations below are simplified if, for 
segment i which has a required acclerat.ion greater than the 
maximum, the transition with the higher of the two velocities 
is designated "hi" and the one with the lower is designated 
"lo"; then 
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The transition distance can then be calculated as 
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The final step in the translational portion of the al- 
gorithm is calculation of the minimum translational times for 
the segments. These times will later be compared with ro- 
tational times to determine if either needs to be adjusted to 
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synchronize the translational and rotational motions. The first 
step in the calculation of the translational times is calcula- 
tion of the highest velocity reached in the segment as 
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The time can then be calculated as 
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4.3.2. Rotational Trajectory. 

The object of the rotational portion of the algorithm is 
to determine for each segment a unit vector £ and an angle (J) 
such that a rotation of (p about ^ makes the desired rotational 
change over that segment. The first step in this procedure is 
calculation of a rotation matrix e for segment i which 
describes the rotational motion for that segment. That matrix 
can be calculated from the rotation matrices of the two end- 
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The vector and angle of rotation can then be derived from 
Euler's theorem [31], which states 

-1 ^i^ 

hh-1 “ ® 


= ^cos 4> +^^(l-cos())) +S_^sin(j) 


where S ^ ^ 


J-2 iy 

0 -r. 


s - s 

32 ^23 

r.sinc}), = -i- s. - s. 

-X 1 2 i^3 i3^ 


s - s 
21 ^12 


it can be seen that (since is a unit vector) 


i = j (s -s 

^ L ^32 ^ 


+ (s -s + (s -s )‘ 

23 ^13 ^31 ^21 h2 


s - s. 

32 ^23 

? . = 4 s . - s . /sind) . 

2 ^3 "31 " 


^21 ^12 


COS(J)^ = 


in i 
11 X 

22 \ 

>3 , 

_ ^33 

, .2 


1 

1 


51 
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so (t>. can be calculated as 
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^ /sln^ ' 
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The minimum rotational time can then be calculated as 



(4-44) 


4.3.3. Combination of Trajectories. 

Now the minimum translational and rotational times for all 
the segments are known and the times must be reconciled to syn- 
chronize the translational and rotational motions. If the 
rotational time is less than the translational time, this can 
be accomplished easily because the rotation can be slowed down 
by using 


03 = 03 
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If the rotational time is greater than the translational 
time, the problem is not so simple because the translational 
velocity is not zero at the ends of the segment. The first step 
is to determine if, with the present transition distances and 
velocities, the translational motion can be slowed enough to 
take as long as the rotational motion. This can be checked by 
first calculating the lowest velocity which can be attained 
in the segment as 
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If V- is zero, the translational motion can be slowed down as 
lo * 

much as necessary; if not, the maximum translational time must 
be calculated as 
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1 max 

If that time is less than the rotational time, one or both of 
the transition velocities must be reduced. In the program im- 
plemented they are reduced to the point where the am velocity 
can be reduced to zero; this is not necessarily a minimum-time 
solution but it is time-consuming to obtain the minimum-time 
solution and this situation arises seldom so it should not 
significantly affect the effectiveness of the algorithm. 

If the minimum rotational time is less than the maximum 
translational time but greater than the minimum translational 
time, the translational velocity profile is determined by cal- 
culating the average velocity which must be maintained over the 
segment (excluding the portion in which the arm is accelerated 
from the lower transition velocity to the higher transition 
velocity) as 
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4.4, Summary. 

This chapter has described the algorithms which are used 
in this work to implement motion of the robot's arms. These 
are the groundwork for the procedures which control the robot’s 
walking, which are discussed in the next chapter. 


Chapter 5 

WALKING ALGORITHMS 


5.1. Introduction. 

The robot discussed in this work walks over the terrain 
shown in Figure 2 , The surface is a cylinder on which there is 
a grid of handholds for the robot's arms to grasp. The pro- 
gram allows any of these handholds to be removed to simulate a 
hole, trench, or similar obstacle. It is assumed that these 
obstacles have no height so a leg can pass over them. 

The algorithm used to control the robot's walking is 
basically similar to that of McGhee and Iswandhi [26], It is 
based on the notion of kinematic margin (defined in Section 

5.3.1. ); the algorithm seeks to maximize the value of kinematic 
margin over all the supporting legs so as to maximize the dis- 
tance the robot can move before one of the arms reaches the 
limit of its joints. The algorithm is described in detail in 
Section 5.3.2. 

5.2. Robot Velocity Command Generation. 

The basic input specification to the free gait algorithm 
described in the next section is a robot velocity command 
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vector. This coramand consists of three translational components 
and three rotational components. Only three of those components, 
two translational and one rotational, are specified by the 
operator. The other two degrees of freedom are supplied by 
requiring the robot to remain at a specified height and tangent 
to the cylinder. The height specification supplies a trans- 
lational velocity and the tangency requirement supplies two 
rotational velocities, so with the three operator-specified 
velocities the robot's motion is fully specified. 

The operator-specified velocities are the robot's 
tangential and axial velocities on the cylinder and the velocity 
of rotation of its body. These commands are generated in one 
of two ways, depending on the mode of operation. 

When the system is in Robot Velocity Mode, a three-axis 
joystick is used, and velocity commands are read each time the 
free gait algorithm is executed. Movement of the joystick 
gives the translational components of the robot velocity and 
rotation of the joystick about its axis gives the rotational 
component . 

When the system is in Robot Destination Mode, the velo- 
city commands are computed from a stored trajectory. This 
trajectory is calculated from a sequence of robot positions and 
orientations entered on entry into Destination Mode. The pro- 
gram first calculates the minimum translational and rotational 
times between each pair of points and determines which of those 
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two times is greater. The greater of the times is used to 
compute the translational and rotational velocities required 
to move between those points in that time. Those velocities 
are then stored and constitute the trajectory for that sequence 
of points. 

5.3. Free Gait Algorithm. 

A flow chart of the walking algorithm is shown in Figure 
9. As mentioned above, the key concept of the free gait 
algorithm is that of kinematic margin. That concept is defined 
in Section 5.3.1., and its use in the algorithm is described in 
Section 5.3.2. 


5.3.1. Kinematic Margin. 

The kinematic margin of an arm grasping a handhold is 
defined as the distance the base of that arm could travel in 
the direction of its current velocity before a joint of that 
arm reached the limit of its travel. The smallest kinematic 
margin of the three arms is thus the distance the robot could 
travel in its current direction before it had to stop because 
of arm limits. The algorithm seeks to maximize the value of 
this smallest margin because that will maximize the distance 
the robot can travel in its current configuration, thus maxi- 
mizing the chance that it will be able to find another con- 
figuration in which to continue. 
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The most accurate method of calculating kinematic margin 
would be to use knowledge of the three-dimensional reachable 
space of the arm. The boundaries of that space are not easily 
described, though, so two-dimensional limits are used in this 
work. Figure 10 shows the limits of the arm’s reach in both 
the tangential and axial directions on the cylinder. These 
limits can be used to determine the approximate shape of the 
reachable area of the cylinder, which is shown in Figure 11. 

This area is not easily described, so an inscribed circle 
centered at the base of the arm is used in the program to cal- 
culate kinematic margin. 

The calculations for kinematic margin assume that the base 
of the arm will continue at its present velocity indefinitely. 
The kinematic margin is then the length of a vector in a direc- 
tion opposite to the arm base velocity from the current handhold 
to an intersection with the boundary of the reachable area (see 
Figure 12) . The next section describes the use of the kinematic 
margin in the free gait algorithm. 

5.3.2. Foot Lifting and Placing. 

The first step performed by the algorithm is a check to 
see if one of the arms is moving from one handhold to another. 

If one is, the algorithm calculates the time elapsed since the 
arm was lifted. This elapsed time is used to determine from the 
stored trajectory of the arm its desired velocity, v^, and 


59 




Figure 10, Arm Extension Limits for Vehicle at a 
Fixed Height (52 in.) on Cylinder. 

(a) Tangential Limits, (b) Axial Limits. 



r Figure 11. Actual and Programmed Reachable Areas for 

Arm of Vehicle on Cylinder 

j 
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Reachable Area 



Figure 12. Kinematic Margin Definition Showing Distance Arm 
Base Can Travel at Present Velocity before Arm 
Reaches Limit 


desired position, That velocity and position information 

is then used with the current actual position, to calculate 

a velocity command, v , for the arm, as 


where At is the time interval between successive executions of 
the free gait algorithm. Thus the command is the sum of the 
desired velocity and a correction velocity intended to correct 
the position error in the time interval At. The cotLmand is then 
executed and a check is made to see if the arm is at the ter- 
minal point of its trajectory. If not, the algorithm moves on 
to calculate velocity commands for the supporting arms; if so 
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Path of End Effector 



Figure 13. Typical Arm Handhold-to-Handhold Trajectory 

the arm is sent a command to grasp the handhold. 

If all of the arms are grasping handholds, the algorithm 
calculates the kinematic margin for each of the arms. The 
algorithm then checks to see if the minimum kinematic margin 
over the three arms is less than some operator-determined 
threshold value. A threshold is used to prevent the arms from 
lifting and setting down more often than necessary, which would 
consume more energy than required. If the minimum kinematic 
margin is larger than the threshold, the algorithm moves on to 
calculate velocity commands for the supporting arms; if the 
minimum kinematic margin is smaller than the threshold, the 
algorithm determines which of the reachable handholds for the 
arm with the minimum margin has the greatest kinematic margin. 

It then calculates (as described in Section 4.3.) a trajectory 
(as shown in Figure 13) for the arm to reach that handhold and 
lifts the arm. 

The final step in the free gait algorithm is calculation of 
the velocity commands for the supporting arms, which are simply 



Che opposites (in the vector sense) of the desired arm base 
velocities, which are determined by the desired robot velocity. 
The algorithm terminates when a stop command is received (for 
Robot Velocity Mode) or the terminal position is reached (for 
Robot Destination Mode) . Control is then returned to a super- 
visory program. 

5.4. Summary. 

This chapter has described the operation of the algorithms 
used to control the robot locomotion. The next chapter dis- 
cusses the performance of the simulation which uses those al- 
gorithms . 
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Chapter 6 

RESULTS AND CONCLUSIONS 


6.1. Simulation Results . 

The algorithms described in Chapters 4 and 5 have been 
implemented on a PDF 11/45 minicomputer with a Vector General 
graphics display. All of the routines are written in Fortran 
except for a few matrix manipulation subroutines, which are 
written in assembly language. Two photographs of the display 
which were taken during program execution are reproduced in 
Figures 14 and 15. 

The approximate maximum speeds for the robot with respect 
to the cylinder are 3 in/sec for translational motion and 0.05 
rad/sec for rotational motion. The rotational speed seems slow, 
but at that speed the end of the arm at full extension is moving 
3 in/sec just as with translational motion. The speed is 
limited to 3 in/sec because at higher speeds the robot’s arms 
cannot maintain positions close enough to the handholds to grasp 
them. This occurs because the simulation requires that the end 
of the arm be no more than one inch from a handhold when it 
grasps it. When the arm’s base is moving rapidly with respect 
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Figure 14. Simulation Display of Vehicle with 
All Three Arms Grasping Handholds 





Figure 15. Simulation Display of Vehicle with Arm 
at Lower Left Moving to New Handhold 
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to the cylinder, the end effector cannot meet the positional 
tolerance, so the arm cannot grasp the handhold. 

For whatever maximum speed of operation is chosen, rapid 
changes in robot velocity create situations in which already- 
selected handholds are no longer practical, or even reachable. 
This occurs because, since handholds are chosen based on the 
current arm base velocities, rapid changes in those velocities 
make the original choices undesirable. This does not create a 
serious problem except in those cases in which the originally 
chosen handholds are unreachable after the change in velocity. 

To deal with this problem, an additional step was added to the 
free gait algorithm. The added step computes the difference 
between the time elapsed since the arm was lifted and the ex- 
pected time for the current arm trajectory which was calculated 
by the trajectory algorithm. If that difference is more than 
one second, a new handhold is chosen based on the new arm base 
velocity. Thus the additional step allows the program to "re- 
consider" its choices based on changing commands from the 
operator. 

As mentioned in Chapter 3, handholds can be removed from 
the terrain to simulate obstacles of zero height such as holes 
or trenches. The robot has successfully crossed trenches as 
wide as four-fifths of the diameter of an arm’s reachable area. • 
This shows that handholds on the terrain do not have to be 
placed closely together for the robot to be able to move over 
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the surface, so long as the robot has an accurate internal model 
of where the available handholds are. 

6.2, Suggestions for Future Work. 

The major weakness of this work is that many of the cal- 
culations (that of kinematic margin, for instance) are made by 
taking advantage of the fact that the terrain is a cylinder. 

An improvement could be made by using the same basic approach 
but using methods of calculations which are applicable for more 
general terrains. This would seem to require use of the 
"reachable volume" of the arm rather than the artificial 
"reachable area" approach. 

Another improvement could be made in the anns themselves. 

As discussed in Section 4,2.2., when one or both of the Jacobian 
matrices for the arms are singular, some velocities are unattain- 
able, which could be a serious problem at times. Use of arms 
with more than six degrees of freedom would allow Jacobian matrix 
singularities to be avoided entirely. 

At any rate, the robot's success in walking over both ter- 
rain with regularly spaced handholds and terrain with obstacles 
seems to confirm the validity of the basic approach used. Al- 
though improvements could be made in the robot's speed and in 
applying the approach used here to more general terrain, this 
work shows that a robot similar to the one described may be a 
practical possibility in the not-too-distant future. 
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APPENDIX : COMPUTER PROGRAMS 


The following pages contain the computer programs 
which were used to implement the algorithms described in 
Chapters 4 and 5. Other programs used in the simulation 
are not included due to space limitations. 


no n n ooooooo 


C 

THIS IS THE hAiN ROBOT CONTROL PROGRAn. 
M.R. PATTERSON 


INTEGER CYL(2t)»24) * TRANS < 12) f ARM i AKM5> TS ( 4 » 3 ) f PR0BC3) rFAKM(3»3) r 
+ JSTK(3) fNUMSEGf APLAb(3) 

REAL TH<6f3) fTHV(6»3) fCTH(7f3) fSTH(7i3) » JO I NTS ( 3 > 8 » 3 ) fEND(3»4>3) » 
f R0BHAT(3r4) » IRBMAT(3r4) f RA£i I AL i AX I AL i HE I GHT f RUBVEL ( 6 ) *RoELC(6) » 
+ TIME r TIMING »OELC( 3) » T AO » AXO i GAO » T A ( 4 ) i AX<4) iGA(4) f£LTIM<4) , 

+ RTAVEL(4) fRAXOEL<4) ,RGA0EL(4) » I NSEbT f REL T IM 
DIMENSION IDISP(3i 10) f £UL£R(3) t TRMAT(3»4) »V£CT1 (3) f VECT2(3) 

COMMON CYL 

COMMON /ARMS/ARMSTS 

COMMON /DISP/FRGB»FARM 


DATA IBEGINf IDtSTr IOELOC» I ACTRL r lEXIT/ ' B 0 A E V 
DATA LTCLR»LT7»LT14»PI/0» *0004001 * 000002 » 3 1 4 1 ti92A5/ 


C 


CALL OGINI 


URITE(5r 100) 

100 FORMAT(///'srH£ OG HAS BEEN INITIALIZED. ENTER 
10 READ<5»101) IREPLY 

101 FORMAT(Al) 

IFdREPLY.NE. IBEGIN) GOTO 10 
C 

C» SET 06 TRANSFORMATION MATRIX AND DISPLAY CYLINDER 
C 

CALL 0GSEN<3) 


CALL OGSEN(l) 

CALL OGSEN(7fO) 

DO 1 I-l»12 
TRANS< I )»0 
1 CONTINUE 
URITE(5»200) 

200 FORMAT <//' CENTER DISPLAY ANGLE (IN DEGREES): ') 

READ<5f201) THETA 

201 F0RMAT(F8.3) 

THETA=TH£TA*PI/180. 

TRANS(1)=»32767 
TRANS(5)=32767tC0S< THETA) 

TRANS<6)“32767*SIN( THETA) 

TRANS(8) = 327673*c(~SIN<THETA) ) 

TRANS! 9 )*32767tC0S (THETA) 

TRANS( 12)»16384 

CALL URC0M(4» 12» TRANS) 

CALL 0GS£N(4) 


C 


B' ' TU begin: ' ) 
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CALL SETHLCi 
C 

C* SET JOINT ANGLES AND AKn STATES TO THEIR INITIAL VALUES. 

C 

DO 2 ARM=»lf3 

THU »ARM)»0.»PI/180. 

TH<2f ARh)a90.»PI/ltiO. 

TH < 3 # ARM ) =-0i . I / 1 bO . 

TH(4f ARM)«0.3|fPI/iy0. 

TH ( 5 • ARM ) a-5 . 4cp I / 1 80 . 

TH(6f ARM)=0.*PI/180. 

THV<6» ARM)=0. 

ARMSTS( 1 »ARM)=0 

2 CONTINUE 
C 

C* SET ROBOT MATRIX. 

C 

URITE(5»300) 

300 FORMATC///' ENTER CENTER COORDINATES (IN POINTS RADIAL AND '» 

+ 'AXIAL) AND HEIGHT Of’ ' / ' i ROBOT BUDY: ') 

R£AD<5f30l ) RADIAL# AXIALfHEIGHT 

301 F0RMAT<3F11 .4) 

ANGLE=25. +5.«RADIAL 
EULER( 1 )=*AN6LE 
EUL£R<2)=90. 

£ULER(3)=0. 

CALL MATEUL(ROBMAT< 1 f 1 J lEULER) 

ROBrtATC 1 i4) = ( lOO.+HEIGHT )*COS< ( 1 80 . -ANGLE ) *PI /I 80 . ) 

R0BMAT(2f 4 )=( 100.+H£IGHT)#bIN( C 1 80 . -ANGLE ) I / 1 80 . ) 
R0BMAT(3 f4)=-9 .25*AXIAL 
C 

C* DISPLAY ROBOT BODY. 

C 

DO 3 ARM=1f3 

CALL TRIG(TH( 1 fARM) fCTH< 1 fARM) fSTH< 1 fARM) ) 

CALL POSIT(CTH< 1 fARM) fSTH< 1 fARM) f J0INTS< 1 f 1 f ARM) f END < 1 f 1 f AKN ) ) 
CALL TRANSM(OfARMfTRMAT) 

CALL MM34 31 ( 4 f TRMAT f VECT 1 f JOINTS ( 1 f 1 fARM) ) 

CALL MM3431 <4fR0BMATfVECT2fVECTI ) 

IDISP( 1 fARM)=VECT2( 1 )»XOO.+0192. 

IDISP(2fARM) =VECT2( 2XC100.+8192. 

I D I SP < 3 F ARM ) = VECT2 ( 3 ) ¥ 1 00 . +8 1 92 . 

3 CONTINUE 
IDISP(lF4)aIDISP(lFl) 

IDISP(2f4)=IDISP(2f1) 

IDISP<3f4)=IDISP<3f1) 

CALL URC0M(64f12f IDISP) 

DO 4 I=1f3 

CALL RDC0M(39f1fFR0B(I)) 

CALL VGSEN(9f4f1) 

4 CONTINUE 
C 

C* DISPLAY ARMS. 

C 

DO 5 ARM=1f3 
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CALL TRANSrttOf ARf1» TRnAT) 

DO 6 l>lf8 

CALL MM3431 (4f TRMATrVECTl » JOINTS( 1 f I »AKM) ) 

CALL MH3431 < 4»R0BMAT» VECT2»VECT1 ) 

ID1SP(1*I)=»VECT2<1) *100. +8192. 

IDISP(2f I)=VECT2<2)*100. +8192. 

IDISP<3f I)ayECT2<3)*100.+8192. 

6 CONTINUE 
IDISP(1»9)=IDISP(1p6) 

IDISP<2i9)=lDISP<2f6) 

IDISP<3f 9>3lDISP(3i6) 

IDISP(lflO)=»IDISP(l»7) 

IDISP<2f 10)»iriISP(2»?) 

IDISP(3» 10)>IDISPC3r 7) 

CALL WRC0h(64f30FlDISP) 

DO 7 1 3 1 F 3 

CALL RDC0M<39f 1 FFAPM(ARrtf I ) ) 

CALL UGSEN<9*10»1) 

7 CONTINUE 

5 CONTINUE 

20 CALL SETriWN<THFTHU»CTHFSTHFJOINTS»ENDfRUBMAI fHEIGHT) 

DO 8 APM*1f3 
DO 9 J=1f6 

THV< J»ARM)*0. 

9 CONTINUE 

8 CONTINUE 

CALL MOVROB<THfTHUfCTHfSTHf JOINTS F END fROBMATfTIMINC fA+ LAG) 
WRITE<5»400) 

400 F0RMAT<///' ENTER "XU" FOR DESTINATION hODEi FOR VELOCITY 

+ ' MODEf "A" FOR ARM'/'*CONTROL MUDEf OR TO EXIT! ') 

30 READ<5f401> IREPLY 

401 FORMAT(Al) 

IF(IREPLY.EQ.IDEST) GOTO 80 
IF(IREPLY.EQ.IVELOC) GOTO 50 • 

IF(IREPLY.EQ.IACTRL) GOTO 40 
IF<IR£PLY.NE.IEXIT) GOTO 30 
CALL U6SEN(16) 

STOP 


« * 

* ARM CONTROL MODE * 

« « 

««*«»*:«*««************ 


40 CALL AKMCTLCTHfTHVfCTHfSTHf JOINTSfENDfRUBMAI ) 
GOTO 20 


« « « ;|c * « « « « ;K :K )|( * ;« * V * « * 
* * 
* VELOCITY MODE * 
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50 TIME=»-0a 

60 CALL ftOCOMaVf If LIGHT) 

CALL WRC0M(l7f IfLTCLR) 

IF<LIGHT.EQ.LT7) GOTO 20 
TIMINC*0.1 
TIME^TIME+TIMINL 
DO 11 ARM»lf3 

IF(ARMSTS(lf AK*h) .EQ.l) GOTO 70 
11 CONTINUE 

CALCULATE ROBOT POSITION AND VELOCITY* 

70 CALL ROBST ( ARM f TH < 1 f ARM ) f THU ( 1 f ARM ) f CTH < 1 f AKM ) f STH ( 1 r AKM ) t 
+ JOINTS < 1 » 1 f ARM ) f END ( 1 f 1 f ARM ) i ROBMAT f ROBVEL ) 

CALCULATE COMMANDED ROBOT VELOCITIES* 

CALL RDCOMC 18f 3f JSTK) 

VELC(1)=FL0AT(ISIGN< IDIM(IABS( JS'jK( 1) + U f 640) f JSTKd) > )/16000. 
VELC(2)=‘FL0AT( ISIGN(IDIM( IABS< JSTK(2)+1) f 640) f JSfK(2) ) )/16000. 
VELC(3)=FL0AT (ISIGNaniMdABSC JSTK(3) + 1 ) f 1 280 ) f JSTK ( 3 ) ) )/640000 
CALL RVEL ( ROBMAT t ROBVEL f HE IGHT f T IM INC f VELC # R VELC ) 

CALL INVM34(IRBMATf ROBMAT) 

CALL ARMS ( TH f THV i CTH f STH f JO INTS r END t ROBMA I’ i HE I GHT f IRBMAT t RVELC f 
+ TIMEf TIMING) 

CALL MOVROBCTHf THV f CTH fSTHf JOINTS f END f ROBMA I f TIMING fAKLAG) 
IF<MAX0<AFLA6(l)fAFLAG<2)fAFLAG(3)).NE*0) GOTO 220 
GOTO 60 


t « 

« DESTINATION MODE « 

« « 

« 4c ««««««« )|c « j|c 4c « X « 


Ct READ SEQUENCE OF POINTS. 

C 

80 DO 12 ARM*1f3 

IF(ARMSTS(lf ARM).EQ.l) GOTO 90 
12 CONTINUE 

90 CALL ROBST(ARMfTH(lFARM)fTHV<liARM)fCTH(:fARM)fSTH(lFAKM)f 
+ JOINTS < 1 f 1 f ARM ) f END <1 f 1 f ARM ) f ROBMAT f ROBVEL ) 

TAO=( <PI-ATAN2(R0BMAT<2f 4) fROBMAKIf 4) ) )4cl80./F*I-25. )/5. 
AX0=R0BMAT ( 3 f 4 ) /-9 . 25 
CALL EULMAT<EUL£RfROBMAT) 

6AO»EULER(3) 

URITE<5f500) TAOfAXOfGAO 
500 FORMATC///' THE ROBOT IS NOU AT 'fJF11.4) 
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URITE(5i600) 

600 F0RMAT<///'StNTER THE NUMBER OK SEGMENTS: ') 

READ<5»601) NUMSEG 

601 FORMAT (13) 

IF < NUMSEG. EGM) GOTO 110 
DO 13 I=*l fNUMSEG-1 
URITE(5f700) I 

700 FORMAT<//'$ENTER POINT 'flXt'l ') 

READ(5»701) TA(I)f AX(I)»GA<I) 

701 F0RMAT(3F11.4) 

13 CONTINUE 
110 URITE<5»800) 

800 FORMAT<//'$ENTER TERMINAL POINT: ') 

READ <5»801) TA(NUMSEG) r AX(NUMSEG) f GA<NUMS£G) 

801 F0RMAT(3F11 .4) 

C 

C* CALCULATE SEGMENT TIMES AND ACCELERATIONS. 

C 

TIM£1=ABS(TA(1 >-TA0)/.25 
TIME2=ABS(AX( 1 )-AX0>/.25 
TIME3=ABS(GA< 1 >-GAO)/l . 

£LTIM( 1 )=AMAX1 (TIME1»TIME2»TIME3> 

RTAUELC 1 )«( TA( 1 )-TAO)/ELTIM( 1 ) 

RAXUEL < 1 ) = ( AX ( 1 ) -AXO ) /ELT I M < 1 ) 

RGAVEL< 1 )*(6A< 1 )-GAO)/ELTIM( 1) 

XF< NUMSEG. EQ, 1) GOTO 120 
DO 14 I S£G=2f NUMSEG 

TIME1»ABS<TA( IS£G)-TA< ISEG-1) >/.25 
TIME2=ABS ( AX ( I SEG ) -AX < I SEG- 1 ) ) / . 25 
TIME3«ABS(GA< ISEG)-GA< ISEG-1 ) )/l . 

ELTIM<ISEG)»ELTIM<ISEG~1)+AMAX1 (TIMEI, TIM£2i TIME3) 
RTAUEL(ISEG)=(TA< ISEG)-TA( ISEG-1 ) )/(ELTIM< ISEG)-ELTIM< ISEG-1) ) 
RAXUEL(ISEG)=(AX( ISEG)-AX( ISEG-1 ) ) / ( ELTIM< ISEG ) -ELTIM < ISEG- 1 ) ) 
RGAUEL(ISEG) = (GA( ISEG)-UA( ISEG-1 ) )/(ELTIM( ISEG)-ELTIM< ISEG-U ) 

14 CONTINUE 


120 TIME»-0.1 

130 CALL RDCOM( 17» IfLIGHT) 

CALL URCOM( 17» IfLTCLR) 

IFCLIGHT .EQ.LT7) GOTO 20 
TIMINC^O.l 
TIME-TIME+TIMINC 
C 

C» CALCULATE ROBOT POSITION. 

C 

DO 15 ARM=1»3 

IF(ARMSTS(1»ARM).E0.1) GOTO 140 
15 CONTINUE 

140 CALL ROBST(ARMrTH(l»ARM),THU<l,ARM)rCTH(l»ARM)fSTH(lfARM)» 
+ JOINTSCl f 1 r ARM) tENIKl » 1 » ARM ) » ROBMAT » ROBUEL ) 

TAA=*< (PI-ATAN2(R0BMAT(2»4) fR0BMAT(l,4) ) ) *180 . /Pl-25 . )/5. 
AXA«ROBMA T < 3 » 4 ) /-9 . 25 
CALL EULMATCEULERfROBMAl) 

6AA*EUL£R<3) 
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c 

C* CALCULATE COMhANnED ROBOT UELOCITIES. 

C 

INSEGT=0* 

ISEG»i 

150 IF<TIME.LT.ELTIM(ISEG) ) GOTO 160 
IF(ISEG«EQ«NUMSEG) GOTO 190 
INSEGT=ELTIrt<ISE6) 

ISEG=ISEG+1 
GOTO 150 

160 RELTIM=TIME-INSEGT 

IFdSEG.EQ.l) GOTO 170 

TA£i=TA<IS£G-l )+RTAOEL< ISEG >4cRELTIh 
AXD=AX< ISEG-1 )+RAXUEL< IStG)*KELTlM 
GAn=GA< ISEG-1)+RGA0EL< IS£G)^K£LTIM 
GOTO 180 

170 TACisTAO+RTAUELd )*RELTIM 
AXD=AXO+RAXUEL( 1 )»R£LTIM 
GAri=GAO+RGAUELd )*R£LTIrt 

180 UELC(l)=RTAVELdS£G)'l-d . /T IM INC ) * ( TAU-TAA ) 

0ELC(2)=RAXUELdSEG) + < 1 ./TIMING) *(AXD-AXA) 

0ELC<3) = (RGAUELdSE6) + ( 1 . /T IM I NO * ( GaD~GAA ) )*PI/180. 

GOTO 210 

190 UELC(1)=< 1 ./TIMING) *<TA(NUMSEG)-TAA) 

VELC(2) = ( 1 ./TIMING) =*t<AX(NUMSEG)-AXA) 

0ELC(3)=< 1 ./TIMINC)*<GA<NUMStG)-GAA)*Pl/ibO. 

IF( (ABS(VELCd ) ) .GT. 1 . ) .OR. (ABS(UELC(2) ) .GT. 1 ) .OR. 

+ CABS<VELC(3) ) .GT. 1 . ) ) GOTO 210 
WRITE<5>900) 

900 FORMAT<///' TERMINAL POSITION REACHED.') 

GOTO 20 

210 CALL RUEL(ROBMAT»ROBUELfHEIGHT.TIMING»UELC»RUELC> 

C 

Ct MOVE ROBOT. 

C 

CALL INUM34dRBMATfR0&MAT) 

CALL ARMS(TH»THUf CTHfSTHf. rJl NTS » END fROBMATr HEIGHT dRBMATFRVELCr 
+ TIMEdIMiNC) 

CALL MUUROB ( TH f THV, CTH r STH r. JOINTS f END pROBMATf TIMING f AFLAC) 
IF(MAX0(AFLAG(1)»AFLAG<2)»AFLAG<3)).N£.0) GOTO 220 
GOTO 130 
C 

220 DO 16 AKM=1f3 

IF(AFLAG(ARM) .EQ.O) GO ''0 16 
URITE(5f 1100) AFLAC (ARM) farm 

1100 FORMATC///' JDINT'fI2f' OF ARMdI2»' IS OUT OK LIMITS.') 

GOTO 20 
16 CONTINUE 
C 

END 
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c 

C THIS subroutine CONTROLS THE ARMS' MOTIONS WHEN THE RUt«OT IS WALKING 
C 

C M.R. PATTERSON 
C 

c 

SUBROUTINE ARMSC THi THU»CTH^S fHr JOINTSf ENDt ROBMA I » HE IGHT f I KBMA T f 
^ RUELCfTIMEfTIMINC) 

c 

INTEGER ARM f ARMSTS < 4 f 3 ) f TSEGCT f RSEGCT f TERFLG f HOHLH ( 2 ) f 
+ HriHL0S<2F48) fNUMSEG 

REAL TH(6f3) fTHU<6f3) fCTH<7f3) fSTH(7f3) f JOINTS < 3 f 8 f 3 ) FENn(3F4F3 ) f 
+ ROBhAT(3i4 ) fHEIGHTfIRBMAT(3f4) fRVELC(6) f TIME f T IMINC f INTIME f 
+ ELTCMEfEXTEND(3f4) f EL TT 1M < 36 ) f TACC ( 3 f 36 ) f INTUEL<3f36) f 
+ INT?0S(3f36) fELRTIM< 12) fRACC(12) f INRP0S<9f 12) f ROTVEC < 3 f 1 2 ) f 

+ TE*\P0S(3f4) fEXTUEL(6) f EX I SEG f PO INTS < 3 f 4 f 6 ) f UEL < 6 ) 

DIMENSION TRMAT(3f4) fOTRMAT(3f4) 

C 

COMMON /ARMS/ ARMS IS 

COMMON /PATH/INTIME f TSEGCT f ELTTIM f TACC f INTUEL f INTPOS f RSEGCT f 
+ ELRT I M F RACC f I NRPOS f ROTUEC f TERPOS 
C 

r* 

Ct FIND ARM IN AIR* 

C 

DO 1 ARM=1f3 

IF(ARMSTS(1fARM) *EU*0) GOTO 10 
1 CONTINUE 
GOTO 30 
C 

C« ARM IS IN TRANSFER PHASE. 

C 

10 ELTIME=TIM£-INTIME 
FLAG=»0. 

IF<(ELTIME-ELTTIM<TSEGCT)).LT.1.) goto 20 
FLAG=1. 

GOTO 40 

20 CALL TRANSM(OfAPMfTRMAT) 

CALL MM3434<0TRiiAT f ROBMAT f TRMAT ) 

CALL MM3434(EXTENDfOTRMATfEND< 1 F 1 FARM) ) 

CALL VELCOM<EXTENDfTSEGCTfELTTIMfTACCf INTUELf INTPOSfRSEGCTf 
+ ELRTIM fRACCf INRPOSfROTVECf TERPOS fELTIMEfTIMINCfEXTVELf TERFLG) 
IF(TERFLG.EQ.O) GOTO 60 
ARMSTS(1fARM)=1 

c 

C* ALL ARMS ARE SUPPORTING* 

C 

30 EXSGMN=99. 

DO 2 ARM=1 f3 

CALL £XIST(ARMfROBMATfHEIGHTfRUELCf ARMSTS(2fARM) fEXISEG) 
IF(EXISEG.GE.£XS'GMN) GOTO 2 
EXSGMN*EXISEG 



fl£SAft(i = AKT1 

2 CONTINUE 

IF(EXS6MN.GT.l. ) GOTO ^0 
AftrtaMESARM 
C 

Ct CALCULATE NEW TRAJECTORY. 

C 

AO CALL TRANSM(0#ARMfTR«AT> 

CALL MM3434 ( OTRMAT » ROfc»MA 1 » TRMAT ) 

CALL MM3434(EXTEND»0TRMArrEND( 1 » 1 » AKN ) > 

CALL HDHOLIK ARM# EXTEND fROBMAT* HE IGHTrHDHLIiSfNOHHS) 

IF(NOHHS.EQ.O) GOTO 70 
EXSGMX»-1. 

DO 3 I=»1»N0HHS 

CALL EXISTCARMfROBMAV f HEIGHT#RUELC»HDHLnS( 1 » 1 > tEXISKG) 
IF(EXISEG.LE.EXSGMX) GOTO 3 
EXSGMX=EXIS£G 
MXESHH=I 

3 CONTINUE 
IF(FLAG.EQ.l. ) GOTO SO 
IF(EXSGMX.LE.EXSGMN) GOTO 70 

50 HDHLD< 1 )=HDHLDS( 1 fMXESHH) 

HDHLD ( 2 ) =HDHLDS ( 2 # MXESHH ) 

C 

CALL HHDF*TH< ARM# OTRMAT# HDHLD# POINTS #NUMSEG) 

DO 4 l3l»4 
DO 5 J»l#3 

TERPOSC J# I )=POINTS( J# I »NUMSEG) 

5 CONTINUE 

4 CONTINUE 

CALL ARMPTH ( EXTEND »NUMSEG# POINTS # TSEGCT # ELTTIM » TACC # INTUEL# INTPOS# 
+ RSEGCT»ELRTIM#RACC# INRPOSfROTUEC) 

C 

ARMSTS( 1 #ARM)*0 
INTIME=TIME 
GOTO 70 
C 

C* MOVE TRANSFER PHASE ARM. 

C 

60 CALL AOEL<ARM#ROBMAT#IRDMAT#RVELC#EXTUEL#OEL) 

CALL SOLUE ( TH ( 1 # ARM ) # THU ( 1 # ARM ) # CTH < 1 » ARM ) # S TH <1 # ARM > # 

+ JOI NTS ( 1 » 1 # ARM ) # END ( 1 # 1 # ARM > # VEL ) 

C 

C* MOVE SUPPORT PHASE ARMS. 

C 

70 DO 6 ARM=1#3 

IF<ARMSTS(1#ARM) .NE.l) GOTO 6 

CALL HLDPOS ( ARM » END ( 1 # 1 » ARM ) » ROBMAT » TIMINL # EXTUEL ) 

C.'jLL AOEL ( ARM # ROBMAT f I RBMAT » RUELC # EXTOEL # UEL ) 

CALL SOLOE(TH< 1 #ARM) # THU U # ARM ) # CTH < I # ARM > rSTHd #ARM) » 

+ JO I NTS ( 1 # 1 # ARM ) # END ( 1 # 1 # ARM ) # LEL ) 

6 CONTINUE 
C 

RETURN 

END 
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c 

c 

C THIS SUBROUTINE RETURNS A LIST OF REACHABLE HANWHULDS FOR* A 
C GIUEN ARM. 

C 

C M.R. PATTERSON 
C 

C 

SUBROUTINE HDHOL0 ( ARM » EXTEND f RUBMAT i HEIGHT » HDHLDS » NOHHS ) 

C 

INTEGER ARM » HtfHLDS (2tAH) f NUHHS » CYL < 25 > 20 » ARMSTS < 4 > 3 ) 

REAL £XTENti(3»4) »R0BMAT(3r4) > HEIGH T » NU » SN » ES > HU » TRM33 < 9 » 3 ) » 

+ TRM31 (3»6) 

DIMENSION XYZ(3) » TA(2) r TACHH<2 ) fTAHH(2) fTAKOBCO) 

C 

COMMON CYL 
COMMON /ARMS/ARMSTS 
COMMON /ARMPAR/NOrSNfESfHU 
COMMON /TRMATS/TRM33r TRM31 
C 

DATA PI/3.14159265/ 

C 

C 

C« CALCULATE ARM BASE COORDINATES IN CTfA). 

C 

CALL MM3431(4fR0BMATFXYZf TRM31 (1 fARM) ) 

CALL TACALC(RUBMATfXYZfTA) 

CALL TACALC ( ROBMAT f EXT END < 1 f 4 ) f TACHH ) 

C 

C* DETERMINE HANDHOLD LIST. 

C 

CENDST=SQRT ( ( 100 . +HEIGHT-NO ) *:<t2+TRM3 1 ( 1 f 1 ) ) 

ANGLE=ATAN( TRM31 ( 1 f 1)/< 100. +HEIGHT-NO) ) 

CRANGL=*< ( 100. +HU)**2+CENDST*:<t2-(2.:|cES)**2)/(2 .4cCENDST*< 100, +HU) ) 
RANGLE=s ATAN < SORT ( 1 . -CRANGL:«c*2 ) /CRANGL ) 

STANGL=»7RM31 ( 1 f 1 ) / ( 100 . +HU) 

TANGLE=ATAN(STANGL/SORT< 1 .-STANGL*«2) ) 

RADIUS^ ( ANGLE+RANGLE-TANGLE ) * ( 1 00 . +HU )/9 . 25 
C 

INDEX=1 
DO 1 I«1f 25 
no 2 J=1 f24 

if(cyl<IfJ).ne.i) goto 2 

IF( (ARMSTS(2fARM) .ED. I) .AND. (ARMSTS(3fARM> .EU. J) ) GOTO 2 
TAHH(1)=FL0AT(I) 

TAHH(2)=FL0AT( J) 

IF(ABSCTAHH(1)-TA<1) > .GT.RADI J) GOTO 2 
IF(ABS(TAHH(2)-TA(2) ) .GT.RAD JS) GOTO 2 
HHRAD2=(TAHH(1 )-TA< 1 ) ) 3*:4:2+ (7 AHH ( 2 ) -TA < 2 ) ) **2 

hhrad=sqrt:hhrad2) 

IFCHHRAD.GT. RADIUS) GOTO 2 
IF(HHRAD.LT, (SN+2. )/9.25) GOTO 2 
CALL TACALC(ROBMATfROBMAT C 1 f4) fTAROB) 


DRB=SURT( (TA(1 )-TAROB( 1 ) )**2+<TA(2)-TAR0B<2) )««2) 
DHH=^HHRAO 

DTPR0D-(TA<1 )-TAROB< 1 ) ) :*c(TAHH< 1 >-TA< 1 ) ) + 

+ <TA(2)-TAR0B<2) ) * ( TAHH ( 2 ) -TA ( 2 ) ) 

IF( (DTPROD/DRB/tiHH) .LT,-0.707> GUTO 2 
A12*<TAHH(1)-TACHH< 1 ) ) **2+ <TAHH<2)-TAt;HH<2) 
B12»<TACHH<1)-TA< 1) )»*2+<TACHH(2>-TA<2) )**2 
C12=HHRAD2 
A1“SQRT(A12) 

Bl=SC4RT(B12) 

C1=HHRAD 

COSB=»(B12-C12-A12)/<-2.»Al»Cl ) 
C0SC»(C12-A12-B12)/<-2 ,:<ca1»B1) 
IF<(COSB*GT.O,)*AND.<COSQ,GT.O.>) GOTO :0 
IF(C1*LT. (SN+2. )/9.25) GOTO 2 
GOTO 20 

10 DIST=C1*SQRT< 1 ,-C0SB»Jr2) 

IF<DIST.LT. (SN+2* )/9,25) GUTO 2 
20 A22=A12 

B22*(TACHH(1)-TAR0B(1> ) *«2+ < TACHH< 2 ) -TAROB < 2 ) ) **2 
C22=(TAHH<1 )-TAROB< 1 ) ) **2+( TAHH(2)-TAR0B< 2) 

A2=*A1 

B2*SQRT(B22) 

C2«SQRT<C22) 

COSB=* ( B22-C22- A22 ) / ( -2 ^ *A2*C2 ) 
C0SC=(C22-A22-B22)/(-2t,*A2*B2) 

IF< (COSB.GT.O* ) .AND* (COSC.GT.O. ) ) GOTO 30 
IF(C2.LT*(TRM31(l>l)-SN-2.)/9.25) GOTO 2 
GOTO 40 

30 DIST=*C2*SQRT < 1 . -C0£B*»2 ) 

IF(DIST*LT.<TRM31C1f1)-SN-2.)/9*2S) GOTO 2 
C 

40 HDHLDS<1fINDEX)=1 

HDHLDS(2f INDEX)=»J 
INDEX»INDEX+1 
2 CONTINUE 
1 CONTINUE 
C 

NOHHSaINDEX-1 

C 

RETURN 

END 
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c 

c 

C THIS SUBROUTINE CALCULATES THE EXISTENCE SEGMENT FOR A GIVEN 
C HANDHOLD AND ARM. 

C 

C M.R. PATTERSON 
C 

. 

C 

SUBROUTINE EX I S7 ( ARM r ROBMA f f HE I GH7 » RVELC » HNDHLT» f EXISEG ) 

C 

INTEGER ARMfHNDHLD(2) 

REAL R0BMAr(3f^)/HEIGHT»RVELC(6)#EXlSEGpN0»SNfESfHWf TRM33<9i3) » 
+ TRM31(3f6) 

DIMENSION XYZ(3) fXYZ0(3»2) r TA ( 2) f TAa< 2 f 2) »XYZV(3) f TAV<2) f 
+ VXY0(2f 2) #TAHH(2) fVXYHH(2> ?VECT(3) 

C 

COMMON /ARMPAR/NOiSNfES»HW 
COMMON /TRMA7S/TRM33f TRM31 
C 

DATA PI/3.14159265/ 

C 

C 

C:« CALCULATE ARM BASE POSITIONS IN (TtA). 

C 

CALL MM343 1 ( 4 f ROBMAT fXrZf TRM3 1 ( 1 » ARM ) ) 

INDEX=1 
DO 1 I=li3 

IFCI.EQ.ARM) GOTO 1 

CALL MM3431 ( 4 f ROBMAT . XYZO C 1 » INDEX ) f TRM31 ( 1 . 1 ) ) 

INDEX=2 
1 CONTINUE 
C 

THa A7 AN2 < XYZ <2) p XYZ ( 1) ) 

CALL TACALCCROBMATfXYZfTA) 

CALL TACALC ( ROBMAT F XYZO ( 1 f 1 ) f TAO( 1 f 1) ) 

CALL TACALC(ROBMATfXYZO(1f2) f7A0<1f2) ) 

C 

Ct CALCULATE ARM BASE VELOCITY IN (TfA). 

C 

CALL MM3431(3fR0BMArFVECTFTRM3l(lFARM) ) 

^ XYZV(1)=RVELC(5)*VECT<3)-RVELC(6>«VECT(2)+RVELC(1) 
XYZV<2)=RVELC(6)5|:VECTa)“RVELC(4)»VECT<3)+kVELC<2) 
XYZV(3)»RVELC(4)*VECT(2)-RVELC(5)*VF.CT<1)+RVELC(3) 
TAV<1)*(SIN(TH)*XYZV(1 )-COS(TH)»XYZV<2) )/9.25 
TAV<2>=*-XYZV(3)/9.25 
C 

Ct CONVERT POINTS TO (VXfVY). 

C 

PHI=ATAN2(TAV(2) f TAVa ) ) 

DO 2 I»li2 

VXYO(lFl)=SlN(PHn4c<TAO<lFl>-TA(l) )-CUS(PHl))|f(TAO<2Fl)-TA<2) ) 
IF(VXYO<1fI) .EQ.O.) VXY0aiI)»1.0E-8 

VXY0C2Fl)»C0S<PHI)«(TA0a F I)-7A(1) )+SIN(PHI)*(TA0<2 fI )-7A.<2) > 
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2 CONTINUE 

TAHh ( 1 ) =ELUA V ( HNDHLD < 1) ) 

TAHH ( 2 ) =FL0A7 ( HNtiHLD ( 2 ) ) 

UXYHH(l)=SIN(PHI)J<f<TAHH(U-TAa))-CUS<PHI)*(TAHH(2)-TA<2)) 
VXYHH(2)=*C0SCPHI)*(TAHH(n-TA< 1 ) ) +SIN(PHI ) * ( TAHH< 2) -TA < 2) ) 

C 

C* CALCULATE THE FOUR ARM LliilTS. 

C 

YLIM1=UXYHH(1 )/UXYO< 1 » l)J«cUXY0(2» 1 ) 

IF< ( (VXYHHd )4cUXYO( 1 > 1) ) . L T . 0 . ) . OR . (UXYHHC2) YLIMl ) ) YLINla-99. 
C 

YLIM2=VXYHH(1 )/VXYO< 1 »2)*UXY0(2i2) 

IF< ( (UXYHH( l)*UXYO( 1/2) ) .LV .0. ) . OR . ( UXYHH ( 2 ) .LT. YLIM2) ) YLIM2=-99. 
C 

RA01a(SN+2. )/9.25 
IF(Ai«S(UXYHH(l) ),Gr.RAlH) GOTO 10 
YLIH33SQRT(RAD1**2-UXYHH(1 )**2) 

10 IF( CABS(UXYHH(1 ) ) .GT.RADl ) *0R. (UXYHHC2) .LT.O. ) ) YLIM3=-99. 

C 

CENDST=SQRT( ( 100 . +HEIGHT~NO ) **2+TRH31 ( 1 / 1 ) **2 ) 
ANGLE=ATAN(TRM31<1/1)/(100.+HEIGHT-NU)) 

CRANGL-( (100.+HU)*)(c2+CENriST**2-(2.*ES)4f^2)/(2.:<:CENDST*(100.+HW) ) 
RANGLE=ATAN ( SORT ( I . -CRANGL**2 ) /CRANGL ) 

STANGL=»TRM31 (1 »1)/<100.+HU) 

TANGLE=ATAN(STANGL/SQRTa .-STANGL2*t*2) ) 

RAH2» ( ANGLE+RAN6LE-TANGLE ) * ( 1 00 . +HW ) /9 . 25 
YLIM4 = -S0RT(RAD2**2-UXYHHa)**2) 

C 

Ct CALCULATE EXISTENCE SEGMENT* 

C 

EXISEG=UXYHH(2)-AMAX1 (YLIMl /YLIM2/YLIM3rYLIM4) 

C 

RETURN 

END 
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c 

c 

C THIS SUbROUTIME UALCULAfES 1 HE PUINTS THROUGH UHICH AN ARM CAN 
C PASS FOR A TRAJECTORY UHXCH WILL TAKE IT TO THE GIVEN HANOHOLD. 
C 

C M.R. PATTERSON 
C 

c 

SUBROUTINE HHDPTH ( ARM i OTRMA T » HNDHLU » PO I NTS r NUMSEG ) 

C 

INTEGER ARMf ARMSTS(4fi) fHNDHLr»<2) fNUMSEO 
REAL 0TRMAT(3i4) f P0INTS(3»4»6) 

DIMENSION £ULER<3) 

C 

COMMON /ARMS/ARMSTS 

r* 

DATA PI/3. 14159265/ 

C 

C 

IND£X=1 

C 

IF(ARMSTS<l»ARM).NE.l) GOTO 10 
ANGLE=»25 ♦ +3 . ^ARMS VS ( 2 ? ARM ) 

POINTSC 1 r4f INHEX)=il03.*C0S( ( ISO . -ANGLE > *PI/1 BO. ) 

P0INTS<2> 4 » INDEX) = 103. J*cSIN( (lao. -ANGLE XPI/iaO. ) 

POINTS < 3 » 4 f INDEX ) =-9 . 25JTARMSTS ( 3 » ARM ) 

EULER(1)=-ANGLE 

E0LER(2)=90. 

EULER(3)=0. 

CALL MATEUL(POINTS(lflfINDEX)^EUL£R) 

IND£X=INDEX+1 

C 

10 ANGL£*25.+S.*HNDHLD<1) 

POINTSa »4f INDEX) »103.»C0S( <lBO.-ANjLE)#Pr/l 80. ) 

POINTS < 2# 4 f INDEX >*103. «SIN< < 180. -ANCLE) »PI/ 180. ) 

POINTS ( 3 » 4 » INDEX ) »-9 . 25*HNUHLD ( 2 ) 

eULER<l)»-ANGLE 

EULER(2)*90. 

£ULER(3)»0. 

CALL MATEUL<P0INTS(1 r 1 t INDEX ) t EULER) 

INDEX^INDEXT'l 

C 

P0INTSai4r INDEX) = 100. «COS< a80.-ANGLE)»Pl/ia0. ) 

P0INTS<2f4f INDEX) = 100.:tSlN( ( 180 . -ANCLE ) *PI/ 180 . ) 

POINTS <3 » 4, INDEX )=-9.25*HNDHLD<2) 

EULERa>*-ANGL£ 

EULER(2)*90. 

EULeR<3)*0. 

CALL MATEUL(POINTSa r 1 • INDEX ) »£ULER) 

C 

ARMSTS ( 2 > ARM ) »HNDHLD < 1 ) 

ARMSTS(3»ARM)=HNDHLU<2) 

AR.1STS(4f ARM)=0 
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NUHSEG=INDEX 

RETURN 

END 


c 

c 

C THIS SUBROUTINE CALCULATES THE TIMES AND ACCELERATIONS REQUIRED TO 
C MOVE AN ARM THROUGH A GIVEN PATH, THE TIMES AND ACCELERATIONS 
C ARE CALCULATED SUBJECT TO MAXIMUM ACCELERATION AND VELOCITY 

C CONSTRAINTS, 

C 

C M.R, PATTERSON 
C 

c 

SUBROUTINE ARMPTH ( END f NUMSEG » POINTS * TSEGCT f ELTTIM f TACC f I NTVEL i 
+ INTPOSfRSEGCTfELRTIMfRACCf INRPOSfRQTVEC) 

C 

INTEGER NUMSEGfTSEGCTfRSEGCT 

REAL END<3»4) »P0INTS<3» 4»6) f ELTTIM<36 ) fTACC(3f 36) » INTVEL<3i36) i 
+ INTP0S<3f 36) »ELRTIM<12) fRACC(12) »INRP0S(9»12) fR0TVEC<3f 12) f 
+ TRVEL(7 ) fTRTIME(7) i TRDIST ( 7 ) » USX ( 6 > f USY ( 6 ) » USZ < 6 > f TSEG TM ( 6 » 5 ) f 
+ TSEGAC(6f5)fRSE6AC(6) fRVEC(3f 6) f TRTMIN ( 6 ) f ROTMIN ( 6 ) » ERRMAX f 
+ MAXTRVfMAXTRAfMAXROVfMAXROA 

DIMENSION SX(6) fSY(6> fS2(6) F DSEGC6) fUNACC(7) fC0SB(7) fTRDMAX(7) F 
+ SQTRV(7> fREQSGA<6) fCHANG£<6) F DINSEGC6) F DMATCH ( 6 ) f SGVMX2 < 6 ) f 
+ SEGVMX < 6 ) F RO fMAT ( 3 f 3 > f ROTDIF ( 3 ) f PHI C 6 ) » ROTSUM ( 3 ) 

C 

COMMON /PTHP AR/ERRMAX f MAXTRV f MAXTRA f MAXROV f MAXROA 
C 
C 

NUMTRaNUMSE6+l 

C 

C* CALCULATE SEGMENT VECTORS AND UNIT VECTORS, 

C • * 

SX ( 1 ) -POINTS < 1 F 4 F 1) -END < 1 f 4 ) 

SY<1)=POINTS<2f4f1 )-£ND<2f4) 

S2(l)*POINTS<3F4f 1)-END(3 f4) 

IF(NUMSEG,LT,2) GOTO 10 
DO 1 I=2fNUMSEG 

SX(I)*P0INTS(1f4f I)-P0INTS(1f4.I-1) 

SY(I)»P0INTS(2f4f I )-P0INTS(2f4fI-1) 

SZ(I)=P0INTS(3f4f I)-P0INTS(3f4f I- l > 

1 CONTINUE 
C 

10 DO 2 I»1fNUMSEG 

DSEG ( I) =SQRT ( SX ( I ) »:ic2+SY < I ) »«2+SZ ( I ) «*2 ) 

IF(DSEG(I).GE.1.0E-6) GOTO 20 
DS£G<I)=0, 

usxa>*o, 

USY(I)=0, 

USZ(I)=0, 

GOTO 2 

20 USX ( I ) =3X < I ) /DSEG < I ) 

USY<I)«SY<I)/DSEG(I) 

USZ(I)*SZ(I)/DSEG(I> 

2 CONTINUE 
C 
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c* calculate transition parameters* 

c 

UNACC(1)»2. 

UNACC(NUMTR)=2. 

IF( (NUMTR-l) .LT.2) GOTO 30 
DO 3 I«2»NUMTR~1 

COSe<I)="<USX<I)*USX(I-l>+USY<I)*USY(I“l)+USZ(I)*USZ(I-l)) 
UNACC<I)=SQRT<2*J(t<l.+C0SB<I) ) ) 

3 CONTINUE 
C 

C* CALCULATE MAXIMUM TRANSITION DISTANCES* 

C 

30 TRDISTC1)=0* 

TRDIST(NUMTR)aO. 

IF( <NUMTR-1) *LT*2) GOTO 4C 
DO 4 I=2»NUMTR-1 

TRDMAX < I ) =4 . *ERRMAX/UNACC ( I ) 

TRrH3T<I)=»AMINl<DSEG<I-l)/2* fDSEG(I)/2* fTRDMAXd) ) 

4 CONTINUE 
C 

C^ CALCULATE SQUARES OK MAXIMUM TRANSITION UELOCITIES* 

C 

40 DO 5 I=1>NUMTR 

SQTRU(I)=2.:<cTRDIST(I)*MAXTRA/UNACC(I) 

5 CONTINUE 
C 

C» CALCULATE REQUIRED SEGMENT ACCELERATIONS. 

C 

50 DO 6 I=*I»NUMSEG 

.F< (DSEG(I)-TRDIST< I+l)-TRDISr( I > ) *NE*0. ) GOTO 70 
IF<(SQTRU(I+1)-SQTRV<I))*NE.0.) GOTO 60 
REQSGA<I)=0. 

GOTO 80 

60 REQS6A<I)=*1.0E+6 

GOTO 80 

70 R£QSGA(I)»ABS<(SQTRV(I + 1)-SGTRV(I) ) / ( 2 * ( DSEG < I) - 
+ TRDISTC I+1)-TRDIST< I ) ) ) ) 

80 IF<I*NE.l) GOTO 90 
SEGAMX=REQSGA<I) 

INSAMX=I 

90 IF(R£QSGA( I) *L£*SEGAMX) GOTO 6 
SEGAMX=REQSGA(I) 

INSAMX=r 

6 CONTINUE 
C 

C* IF GREATEST SEGMENT ACCE'LERATION IS GREATER THAN MAXIMUM ALLOWABLE 
Ct ACCELERATIONf RECALCULATE TRANSITION VELOCITIES AND DISTANCES. 

C 

IF< (SEGAMX-MAXTRA) *L£* 1 .OE-4) GOTO 130 

IF<SQTRV( INSAMX) .GT.SQTRVC INSAMX+1 ) ) GOTO 110 
ILO=INSAMX 
IHI=INSAMX+1 
. GOTO 120 

110 IHI=INSAMX 

IL0=INSAMXK1 
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120 SQTROdHI )»<SQTR^( IL0)+2.#MAXTRA«<DS£G( INSAMX)-TRniST( ILO) ) )/ 
+ (l.+UNACC<IHI)) 

TROIST(IHI)*SQTRV( IHI>»UNACC(IH1)/(2.»MAXTRA) 

GOTO 50 
C 

Ct CALCULATE TRANSITION VELOCITIES AND TXhES. 

C 

130 TRVEL(I)»0. 

TRVEL<NUMTR)»0* 

IF< (NUMTR-1) .LT.2) GOTO 150 
DO 7 I»2rNUMTR-l 

TRVEL < I ) -SORT < SQTRV < I ) ) 

IF<TRVEL(I) .NE.O. ) GOTO 140 
TRTIME(I)=0. 

GOTO 7 

140 TRTIHEC I )=2.*TR0IST( I )/TRVEL( I ) 

7 CONTINUE 
C 

C* CALCULATE MINIMUM SEGMENT TIMES. 

C 

150 DO 8 I=1»NUMSEG 

IF(SQTRV<I) .GT.SGTRV(I + 1) ) GOTO 160 
CHANGE ( I) «1 

IF(SQTRV< I) .EU.SGTRV<I + 1) ) CHANGb< I)-0. 

ILO»I 
IHI’I+1 
GOTO 170 

160 CHANGE(I)=-1. 

IHI*I 
IL0=»I + 1 

170 DINS£Q(I)='DSEG< I )-7RDIST(I)-TRDIST< I + l ) 

DMATCHd ) = <SOTRV< IHn-SQTRV( ILO) ) / ( 2 . *MAXTRA ) 

SGVMX2( I )=MAXTRA*(DINSEG< I )-DMATCH< I ) ) +SQTRV(IHI > 

SEGVMXd )=»S0RT<SGVMX2d ) ) 

IF(SEGVMXd) .LE.MAXTRV) GOTO 180 
SEGVMXd)aMAXTRV 
SGVMX2 < I ) -SEGVMX < I ) 

0PEAK=»DINSEGd)-DMATCHd)-(SGVMX2d)-SQTRV< IHI ) )/MAXTRA 
TRTMINd) = (2.*SEGVMXd)-TRVELdHl)-TRVELdL0) )/MAXTRA+ 

+ DPEAK/SEGVMXd) 

GOTO 8 

1 80 TRTMIN ( I ) =« < 2 . #SEGVMX ( I ) -TRVEL (IHI ) -7 KVEL (ILO ) ) /MAXTRA 

8 CONTINUE 
C 

C» CALCULATE ROTATIONAL MATRIX AND ANGLE AND UNIT VECTOR OF ROTATION. 
C 

CALL MT3333<R0TMAT( 1 # 1 ) f POINTS ( 1 f 1 f 1 ) fEND( 1 f 1 ) ) 

DO 9 I=1fNUMSEG 

IFd.EQ.l) GOTO 190 

CALL MT3333<R0TMAT(1 f1 ) fP0INTS(1 f 1 f I ) fPOINTSCI f1 fI-1) ) 

190 R0TDIF(l)»(R0TMAT(3F2)-R0rMAT(2F3> )/2. 

R0TDIF(2)»<R0TMAT(1 f3)-R0TMAT(3f1) )/2. 

ROTD IF ( 3 ) =« ( ROTMA T < 2 d ) -ROTMAT < 1 f 2 ) ) /2 . 

SINPHI=S0RT(R0TDIF<1 )1c«2+R0TDIF<2)iit»2+R0TDIK(3)!*c*2) 
C0SPHI»(R0TMAT(1 f1 )+R0TMAT(2f2)+R0TMAT .3f3)-1 . >/2. 
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PHI ( I )=ATAN2(SINPHI fCOSPHl ) 

IF(SINPHI.LT.1.0E-6) GOTO 210 
RVEC< 1 f I)aftOTPIF< 1 )/SINPHI 
RVEC<2»I)»R0TDIF<2)/SINPHI 
RVEC<3f I )=R0TtHF<3)/SINPHI 
GOTO 9 

210 IF(COSPHI.LT. 0.9999) GOTO 220 
PHKDaO. 

RVEC(1#I)»0. 

RVEC(2f I)*0. 

RUEC(3>I)*0. 

GOTO 9 

220 ROTSUM < 1 ) = < ROTM AT < 3 > 2 ) +ROTMAT ( 2 > 3 ) ) /2 . 

R0TSUM<2)»<R0TMAT( 1 »3 ) +ROTMAT ( 3 # 1 ) ) /2 . 

ROTSUM < 3 ) » ( ROTMAT < 2 » 1 ) +K0 f MAT < 1 » 2 ) > /2 . 

IF< (ABSCROTSUMd) ) .LT. 1 *0E-6) .AND. ( ABS < ROTSUM < 2 ) ) .LT. 1 .OE-6) ) 
+ GOTO 230 

ALPHA*ATAN2<R0TSUM< 1) fR0TSUM(2) ) 

C0S2B=R0TMAT(3f3) 

IF(ABS(SIN<ALPHA) ) .GT. 1 ,0E-6) SIN2B=R0TSUM < 1 ) /S I N < ALPHA ) 
IF<ABS<COS(ALPHA) ) .GT. 1 .OE-6) SIN2B=R0TSUM < 2 ) /COS< ALPHA ) 

BET A=ATAN2 ( SIN2B f C0S2B ) /2 . 

ROEC ( 1 » I) ^COS < ALPHA ) #S I N < BETA ) 

ROeC < 2 » I ) =S IN < ALPHA ) *S I N ( BET A ) 

R0ECC3f I)=COS(BETA) 

GOTO 9 

230 IF(R0TMAT(3»3) .LT. -0.9999) GOTO 240 
ROEC(1»I)»0. 

RVEC(2»I)=0. 

RVEC(3f I)=l . 

GOTO 9 

240 C0S2A=R0TMAT ( 1 f 1 ) 

SIN2A=R0TSUM<3) 

ALPHA»ATAN2< S IN2A f C0S2A ) /2 . 

ROEC < 1 » I) *COS ( ALPHA ) 

RVEC(2>I)=SIN<ALPHA) 

R0EC(3»I)*0. 

9 CONTINUE 
C 

C* CALCULATE MINIMUM ROTATIONAL TIMES. 

C 

DO 11 I»1>NUMSEG 

ROTMIN < I ) = 2 . *SQRT ( PHI ( I) /MAXROA ) 

11 CONTINUE 
C 

Ct TEST FOR SUFFICIENT TIME FOR ROTATION. 

C 

TSFLMX=0. 

ITSFMX=0 

DO 12 Ial»NUMS£G 

IF(ROTMIN<I).L£.TRTMIN<D) GOTO 12 
TTUL02^AMIN1 < SQTRU( I ) >SQTRU< I + l ) ) 
TRUMN2=«TT0L02-MAXTRAC(DINS£G( I )-DMATCH( I ) ) 

IF(TRUMN2.LE.O. ) GOTO 12 

TRTMAX* ( TRUEL ( J + 1 ) + TRVEL ( I) -2 . tSQRT ( TRUMN2 > ) /MAXTRA 
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IF< (ROThIN( I )-TRTMAX> .L£ .TSf-LMX) GOTO 12 
TSFLMX=*ROTMIN< I ) -TRTMAX 
ITSFMX*! 

12 CONTINUE 
C 

C* IF THERE IS A ROTATIONAL TIME SHORTFALL# RECALCULATE TRANSITION 
Ct UEILOCITIES AND DISTANCES. 

C 

IF(ITSFMX.EQ.O) GOTO 280 

IF<CHANGE(ITSFHX) .EQ.-l. ) GOTO 250 
ILO»ITSFMX 
IHI=ITSFMX*K 
GOTO 260 

250 IHI=ITSFMX 

IuO»ITSFMXFl 

260 IF( (SQTRU< ILO)/MAXTRA) .GT. < DSEG < I TSFMX ) -2 . »TRDI ST ( I LO ) ) ) 

+ GOTO 220 

SQTRU<IHI) = (2.:«cMAXTRA*(DSEG( ITSFMX)-TRDIST( ILO) )“SQTRV< ILO) )/ 

+ (1 .+UNACCCIHI ) ) 

TRDIST<IHl>=SQTRU(IHI)*UNACC(IHI)/(2.J«cMAXTRA) 

GOTO 50 

270 SQTRV<IHI)=MAXTRA*DSEG< ITSFMX) /<l.+UNACCaHI)+UNACC(ILO> ) 

SQTRU(ILO)=SQTRU(IHI ) 

TRrUST<IHl)=SQTRU(IHI)*UNACC(IHI)/(2.»tMAXTRA) 

TRDIST( ILO)aSQTRU(ILO)JttUNACC( ILQ)/<2.»MAXTRA) 

GOTO SO 
C 

C» CALCULATE SEGMENT TIMES AND ACCELERATIONS. 

C 

280 DO 13 I«1»NUMSEG 
TSEGTM<If1)=0. 

TSEGTM<If2)=»0. 

TSE6TM<If3)=0. 

TSEGTM<If4)=0. 

TSEGTM<If5)=0, 

TSEGAC ( I F 1 ) =CHAN6£ ( I ) *MAXTRA 
TSEGAC(If3)=0.' 

TSEGAC ( I F 5 ) =CHAN3E ( I ) *MAXTRA 
IF<CHAN6E<I) .E0.~1 . ) GOTO 290 
ILO=I 
IHI*I+1 
GOTO 310 
290 IHI=I 

IL0«I+1 

310 TMATCH=»<TRVEL< IHI )-TRVEL< ILO) )/MAXTRA 
IF<ROTMIN(I) .GT.TRTMIN(I) ) GOTO 320 

IF<CHANGE<I) .EQ. 1.) TSEGTM ( I f 1) =TMATCH 
IF ( CHANGE <n .EQ.-l. ) TSEGTM (I f 5 ) =7 MATCH 
TSEGTM<If2) = <SEG^^MX(I)-TRVEL<IHI))/MAXTRA 
TSEGAC(If 2)=MAXTRA 
TSEGTM<lF4)=«TSeGTM(I»2) 

TSEGAC( I f4)3-TSEGAC(If2) 

TR£M=*TRTMIN<I)-TSEGTM(If1)-TSEG7M< I f2)-TS£GTM( I f4)-TSEGTM(I f 5) 
IF(TREM.GE. l.OE-6) TSEGTM< I f 3)=TREM 
RSEGAC(I)3(R0TMIN(I)/TRTMIN(I) )»:|t2«MAXR0A ' 
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GOTO 13 

320 TRVAME=(DINSEG<I)-DrtATCH<I ) ) / < ROTM IN < I > -THATCH ) 

IFCTRVAVE.LE* < (HAXTRO+TRVELCIHI) )/2. ) > GOTO 330 
IF(CHAN6E<I) .EQ. 1.) TSEGTMC I r 1 )=THATCH 
IF(CHANGE(I) .EQ*-1 . ) TSEGTH ( I f 5 ) -THATCH 
T234=R0THIN( I )-TMATCH 

TSE6TM < I » 2 ) -T234* ( TRUAVE-HAXTRO > / < TRVEL ( IHI ) -HAXTRV ) 

TSEGAC < I F 2 ) = < HAXTRO-TRVEL ( I H I ) ) /TSEGTM < I f 2 ) 

TSEGTHdf 4) = TSE6TH<I»2) 

TSEGAC < I f 4 ) =-TSEGAC < I » 2 ) 

TSEGTM ( I F 3 ) - T234- TSEGTM < I r 2 ) -TSEGTM ( I f 4 ) 

GOTO 370 

330 IF<TRVAOE.L£.TROEL<IHI)) GOTO 340 

IF(CHANG£(I).EtK 1.) TSEGTM < I v 1 ) -THATCH 
IF<CHANGE(I) .E0,-1 . ) TSEGTMC I fSI-THATCH 
TSEGTM<If2)=‘(R0THIN(I )-TMATCH)/2. 

TSEGACd f2)=2.#(TRVA0E-TRVEL<IHI) )/TS£GTM(I f2) 

TSEGTM<If4> = TSEGTMdf2) 

TSEGAC d F 4 ) —TSEGAC (I f 2 ) 

GOTO 370 

340 IF(TROAVE.LT.TROELdLO) ) GOTO 350 

TSEGTM d F 1 ) = ABS< TROAVE-TROEL < I ) )/MAXTRA 
TSEGTM< I f 3)=R0TMIN ( I )-TMATCH 
TSEGTMd F5)=ASS(TRVAVE-TRVELd + l) J/MAXTRA 
GOTO 370 

350 IF<TRVAVE.LT. <TRVELdL0)/2. ) ) GOTO 360 

IF(CHANG£d) .EQ. 1. ) TSEGTM d f 5 ) -THATCH 
IF< CHANGE (I) .EQ.-l d TSEGTM < I f 1 ) -THATCH 
TS£GTM<If2)-(R0TMIN< I)-TMATCH)/2. 

TSEGAC(If 2>— 2.*(TRO£LdL.O)-TROAOE)/TSEGTM(lF2) 
TSEGTM(lF4)-TSEGTMd f2) 

TSE6AC<If 4>— TS£GACdF2> 

GOTO 370 

360 IF(CHANGEd) ,EQ. Id TSEGTMd f5)-TMATCH 

IF < CHANGE (I) .EQ.-l d TSEGTM d f 1) -THATCH 
T234=R0TMINd )-THATCH 
TSEGTM d F 2 ) =T2343 «cTROAOE/TROEL dLO ) 

TSEGAC(If 2)— TROELdLOl/TSEGTHd ^ 2 ) 

TSEGTMd f 4) -TSEGTM d F 2) 

TSEGAC d F 4 ) — TSEGAC d f 2 ) 

TSEGTMd f 3)-T234-TS£GTM (I F 2) -TSEGTMd f4) 

370 RSEGACd )=MAXROA 

13 CONTINUE 

C 

C* SET UP PATH TRAJECTORY. 

C 

CALL SETPTH < END fNUMSEGf POINTS F TSEGCT F ELTT IM F TACC F INTUELf 
+ INTPOSfRSEGCTfELRTIHfRACCfINRPOSfROTVECfTROELf TRTIMEf TRDISTf 
+ USX f USY F USZ F TSEGTM F TSEGAC fRSEGACfRVECfTRTM IN F ROTH IN) 

c 

RETURN 

END 
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c 

c 

C THIS SUBROUTINE SETS UP THE DATA FOR AN ARM TRAJECTORY^ INCLUDINU 
C TIMESf ACCELERATIONSf UELOCITIESf AND POSITIONS. 

C 

C M.R. PATTERSON 
C 

Ctttt 

C 

SUBROUTINE SETPTH ( END »NUMSEGf POINTS i TSEGCTrELTTlMf TACC» INTUELi 
+ INTPOSf RSEGCT» CLRTiMf RACCf INRPOS f ROTVEC r TRUEL > I RTIMEi TRDIST> 

+ U3X»USYrUS2f TSEGTMfTSEGACf RSEGAC fRUEC»TRTM1N?RQTMIN) 

C 

INTEGER NUMSEGfTSEGCTfRSEGCT 

REAL END(3f 4) f POINTS (3r4F6)-FELTTIM<3d) >TACC(3»36) » INTUEL < 3 f 36 ) f 
+ INTP0S<3f 36 ) fELRTIMC 12) fRACC(12) f 1NRP0S<9f12) f ROTVEC ( 3 f 1 2 ) f 
+ TRVEL(7> FTRTIh£<7) f TRDIST(7 ) fUSX<6) fUSY(6) fUSZ<6) fTS£GTM(6f5) 
+ TSEGAC<6f5) fRS£GAC<6) fKUECC3f6) f1RTMIN<6) fR0TMIN<6) 

C 

C 

C:* SET UP translational PATH. 

C 

ITSEG=1 

ELTTIM<ITSEG)=0. 

tacccifUseg^^o. 

TACC<2f ITSEG)=0. 

TACCC3fITSEG>=«0. 

INTVEL(1fITSEG)=0. 

INTVEL(2fITSEG)=0. 

INTVEL<3fITSEG)=0. 

INTPOS ( 1 F I TSEG ) =£Nn ( 1 f 4 ) 

I NTPOS < 2 F I TSEG ) =END ( 2 f 4 ) 

INTPOS<3f ITSEG)=END<3f4) 

ITSEG=ITSEG+1 
DO 1 I=1fNUMSEG 

IF(I.EQ.l) GOTO 30 

IF(TRTIM£(I).QE.1.0E-6) GOTO 10 
ELTTIM(ITSEG>*£LTTIM< ITSEG-1 ) 

TACC( 1 F ITSEG)=0. 

TACC(2fITSEG)=0. 

TACCv3fITSEG)=0. 

GOTO 20 

10 £LTTIM< ITSEG)»ELTTIM( ITS£G-1)+TRTIME(I ) 

TACC( If ITSEG)=<TRVEL( I )/TRTIME( I) )«(USX( I) - USX( I-U ) 
TACC(2fITSEG) = <TRVEL<I)/TRTIME(I) ))ic(USY(I)“USY( I-l) > 
TACC(3 f ITSEG)=(TRUEL( I )/TRTIME( I ) )*<USZ< D-USZ ( I-l ) ) 

20 INTVEL ( 1 F I TSEG ) =TRV£L < I ) »USX ( I- 1 ) 

I NTUEL ( 2 F I TSEG ) = TRUEL < I ) «USY < I - 1 ) 

INTVEL (3p ITSEG)=TRUEL( I )*USZ(I-1 > 

INTPOSCl . 1TSEG)=P0INTS< 1 f4fI- 1 )-TRDIST(I )<CUSX( I-l) 
INTP0S(2 f ITSEG)=»P0INTS<2f4f I- 1)-TRDIST( I )*USY< I-l ) 
INTP0S(3 f I TS£G)*P0INTS(3f4f I-1)-TRDIST( t ):«cUSZ< I-l ) 
ITSEG=ITSEG+1 
30 DO 2 J=1f5 
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IF(TtJEGTH<I» J).E:Q.0. ) GOTO 2 

£LTIME=ELTTIh< ITbEG-l)-£LTTIM<ITSEG-2) 

IF(ITSEG.EQ42) £LTIME=0. 

ELTTIM<ITSEG)=ELTTIM(ITSES-1 > + TSEGTM< 1 1 J) 

TACCd »XTSEG>=»TS£GAC( I »J)»USX(I) 

TACCC2» ITSEG)=»TSEGAC< If J)»USY( I ) 

TACC(3f ITS£0)-TS£GACaf J)#USZ(I) 

INTVEL< 1 f ITSEG)»lNTVELa » ITSEG-1 ) + TACC (1 1 ITSEO-l)* 

+ ELTIME 

INTVEL<2f ITS£G)=»INTVEL(2f ITSEG-1 ) + TACC(2f ITSEG-l )* 

+ ELTIME 

INTVEL(3f ITSEG) = INTUEL(3f ITSEG-l)+TACC(3f ITSKG-D* 

+ ELTIME 

INTPOS< 1 f ITSEG)=INTPOS( 1 f ITSEG-1 )+INTVEL ( 1 f ITSEG-1 )# 

+ ELTIME+TACCd f I 'SEG-l)*t:LTIME<c*2/2. 

INTP0S<2f ITSEG)*INTP0S(2f ITSEG-1 )+INTVEL(2f ITSEG-1) # 

+ £LTlME+TACC(2f ITSEG-1) #ELTIME»*2/2. 

INTP0S<3f ITSEG)=slNTPOS(3f ITSEG-1 ) + INTVEL(3f ITSEG-1 )« 

+ £LTIME + TACC(3f ITSEG-l ):iteLTIME*5|t2/2. 

ITSEG=ITSEG+1 
2 CONTINUE 

1 CONTINUE 
C 

TSEGCT=ITSEG-1 

C 

C» SET UP POfATIONAL PATH. 

C 

IRSEG=»1 

£LRTIM(IftSEG)=0. 

RACC(IRSEG)=0. 

INRPOSCl f IRS£G> = ENri(lf 1) 

INRPOS(2f IRSEG)=END(2f 1) 

INRPOS<3f IRSEG)=‘ENCK3f 1 ) 

INRPOS ( 4 f I RSEG > =END ( 1 f 2 ) 

INRPOS ( 3 f I RSEG ) =ENCt ( 2 f 2 ) 

INRP0S(6f IRSEG)*END<3f 2) 

INRPOS < 7 f I RSEG ) ==£ND ( 1 f 3 ) 

INRPOS < 8 f I RSEG ) =END ( 2 1 3 ) 

INRP0S(9f IRSEG)=£ND(3f3) 

IRSE6=IRSE6+1 
DO 3 I=lfNUMSEG 

IF(I.EQ.l) GOTO 40 

ELRTIM( IRSEG)»£LRTIM(IRSEG-1 )+TRTIME< I ) 

RACC< IRSEG)=0. 

INRPOS<lf IRSE6)=P0INTS(lf If I-l) 

INRPOS ( 2 f I RSEG ) =P0 1 NTS < 2 f 1 f I - 1 ) 

INRP0S(3f IRSEG)=P0INTS(3f 1 f I-l > 

INRP0S(4f IRSEG)=P0INTS(lf2f I-l ) 

INRP0S(5f IRSEG)=»P0INTSr^'2f I-l) 

INRP0S(6f IRSEG)»P0INTS(3f2f I-l ) 

INRP0S<7f IKSEG)=POINTS< ’ 3f I-l ) 

INRPOS (8f IRSEC)*P0INTS(2f 3f I-l ) 

INRP0S(9f IRSEG)=P0INTS(3f3f I-l ) 

IRCEG»IRSEG+1 

40 ELRTIM< IRSEG)=ELRTIM< IRSEG-1 )+AMAXl (ROTMINC I ) t TRTMIN< I ) ) 
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RACC ( I ftSEG ) =KS£G AC ( I ) 

ROTUECd f IRSEG)-=»RVEC( 1 tl) 
f:JTUEC( 2» IK:SEG)=KVEC(2f I ) 

R0Tl<EC(3f IRSEG)»kVEC(3f I ) 

DO A J=»lr9 

. .RPOS < J f I RSEG ) = INRPOS ( J » I RSEG- 1 ) 
CONTINUE 
IRSEb-IRSEG+1 
CONTINUE 


RSEGCT=*IRSEG-1 

RETURN 

END 


on o n noonnnno 


C%*t% 

THIS SUBROUTINE CALCULATES THE CUHhANDED AKM VELOCITY FROM THE 
TRAJECTORY DATA. 

M.R. RATTER30N 


SUBROUTINE VELCOM l EXTEND » TSEGC f f EL TT I M » TACC f INTVELf I NTPOS » RSEGCT 
+ ELRTIMrRACCr INRPOS r ROTVEC f TERF'OS » EL T IME > T IM INC » VEL r TERFLG > 

INTEGER TSEGCTf RSEGCT r TERFLG 

REAL EXTENDI 3f 4) f ELTTIM(36) » TACC(3f3A) f INTVEL(3f 36) f INTPOS(3f 36) 
+ ELRTIMI 12 ) fRACC( 12) f INRP0S(9f 12) fR0TVEC(3f 12) i TERP0S(3f 4 ) f 
+ ELTIMEf TIMINCf VELC 6) i TTRERR f TTRVEL f TROERR f TROVEL 
DIMENSION VELD(6) fP0SD(3f4) fROTMATISfS) f ERROR! 6) 

COMMON /TERPAK/TTRERRf TTRVELf TROERRf TROVEL 


c* 

c 


c 


c 

c» 


CALCULATE TRANSLATIONAL ERROR. 

RINSGT*0. 

ITSEG^l 

10 IF(ELTIME.LT.ELTTIM( ITSEG) ) GOTO 20 
IF( ITSEG.EQ. TSEGCT) GOTO 30 
RINSGT=ELTTIM< ITSEG) 

ITSEG=ITSEG+1 
GOTO 10 

20 ELSrGT*£LTIME-RINSGT 

VELDI 1 )«INTVEL( 1 f I TSEG ) + TACC ( 1 f I TSEG ) YELSEGT 
VELD(2)»INTVEL(2f I TSEG ) + TACC < 2 f I TSEG ) *ELSEGT 
VELD ( 3 ) * I NTVEL ( 3 f ITSEG ) + TACC ( 3 f ITSEG ) *ELSEGT 
POSD< 1 f 4)=INTP0S< 1 F ITSEG)+INTVEL(1 F ITSEG )*ELS£urF 
+ TACCC 1 F ITSEG'tELSEGT**2/2. 

P0SD(2f4)3INTP0S(2f ITSEG)+INTVEL( 2> ITSEG)*ELSh6T+ 
+ TACC<2 f ITSEG) *£LS£GT»<2/2. 

P0SU(3f4)=INTP0S(3f I 1SEG)+1NTVEL(3f ITSEG)*£LS£CT+ 
+ TACC(3f ITSEG)*CLSEGT**2/2. 

ERROR! 1 )=POSD! 1 f4)-EXTEND! 1 f4) 
ERR0R!2)»P0SD!2f4)-EXTEND! 2f4) 

ERROR! 3)=P0SD!3f4)-£XTEND!3f4) 

GOTO 40 
30 VELD! 1 )=0. 

VELD!2)=‘0. 

VELD!3)=«0. 

ERROR! 1 )=«TERPOS! 1 f 4 )-£XTEiND! 1 f4) 
£RR0R!2)»TERP0S!2f4)-EXTEND!2f4> 

ERROR ! 3 ) = TERPOS ! 3 f 4 ) -EXTEND ! 3 f 4 ) 

CALCULATE ROTATIONAL ERROR. 


C 


( • 






‘c/? 


QOAii, 


Is 

V 
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40 RIN55GT=0. 

IRSEG^l 

50 IF(ELTIrtE.Lr.£LRTIi 1 (IRSEG)) GUTU 60 
IF( IRSEG.EQ.RSEGCT) GOTO 90 
RINSGT=£LRTIM( IRSEG) 

IRSEG=«IRSE6+1 
GOTO 50 
C 

60 ELSEGT = ELTIME-RINS*’jT 

RTINT2=*<ELRTIM<IRSEG)-ELRTIM< IRSEG-1 ) >/2. 

IF<ELSEG1 .GT.RTINT2) GOTO 70 
RVELD=RACC( IRSE6 > <£LSEGT 
PHt«RACC( IRSEG)«tLSEGT*<c2/2. 

GOTO 80 

70 RVELD=RACC( IRSEG)*KTINT2-RACC( IRSEG)*(£LS£GT-RTINT2) 

PHI = RACC< IRSEG))|tRTINT2**2/2. +RALC( IRSE6)*RTINT2*(£LSEG r-RTINT2)- 
+ RACC( IRS£G)*<EL3EG T-KT X NT2 > »*2/2 . 

80 OELD ( 4 ) =*ROELD)|cROTOEC ( 1 f IRSEG ) 

0ELD(5)-RVELn»R0T0EC(2f IRSEG) 

OELD<6)~ROELn«ROTOEC(3i IRSEG) 

CPHI*C0S(RHI ) 

SPHI=SIN(PHI ) 

Rl 1 1CP=R0T0EC( 1 » IRSEG)»ROTVEC( 1 f IRSEG )*< 1 . -LPHI ) 

R221CP=R0TVEC<2f IRSEG ) *ROTVEC ( 2 » IRSEG )*< 1 .-CPHI ) 

R331CP=R0T0EC(3» IRSEG)#K0TVEC<3» IRSEG)*< 1 .-CPHI ) 

R121CP=R0T0EC(1 » IRSEG ) *K01 OEC ( 2 r IRSEG ) ( 1 . -CPH I ) 

Rl31CP=>ROrO£C( 1 » IRSEG )«R0T0EC?3> IRSEG)#< 1 .-CPHI ) 

R231CP-ROTOeC(2r IRSEG ) »ROTOEC ( 3 » IRSEG ) # < 1 .-CPHI ) 

RISP^ROTVECC I f IRSEG)#bPHI 
R2SP=R0TVEC ( 2 » I RSEG ) *SPHI 
R3SP^R0TVEC ( 3 » IRSEG ) *SPHI 
ROTMATC 1 f 1)*R111CP+CPHI 
R0TMAT(2f 1 )=R121CP+R3SP 
R0TMAT<3r 1 )=K131CP-R2SP 
ROTMAT< 1 »2)=f<121CP-R3SP 
ROTMAT ( 2 » 2 ) *R221CP+CPH I 
R0TMAT(3f 2)=R231CP+R1SP 
ROTMATC 1 f3)=R 131 CP+R2SP 
ROTMAT <2 f 3)=R231CP-R1SP 
R0TMAT(3F3)aR331CP+CPHI 

CALL MM3333(P0SD»R0TMAT»INRP0S( If IRSEG) ) 

CALL ROERRCCEXTENDfPOSDf ERR0R<4) ) 

GOTO no 
90 0ELD<4)*0. 

VELD(5)-0. 

0EL0(6)*0* 

CALL ROERRCCEXTENDf TERP0SfERR0R(4) ) 

C 

Ct CALCULATE VELOCITY CQMMANHS. 

C 

110 DO 1 I=i»6 

VEL ( I ) = VELD (I' + (l. /TIMING) ^PERROK ( I ) 

1 CONTINUE 
C 

C* CHECK FOR TERMINAL POSITION. 


94 


TE^FLG=0 

IF<ELTIME.LT.ELTTIM(TSEGCT) ) GUTO 120 

TRERR=S0RT(ERR0K( l)#*'3 + ERROR(2)»J^c2+£RftOR(3)**2 J 
IF(TRERR.GT.TTRERR) GOTO 120 

R0ERR-SCJRT<ERR0R(4)#*2+£RR0R(5>#»2 + ERR0R<6)3»:*2) 
IF(ROERR*GT.TROERR) GOTO 120 
TROEL=SQRT(OEL(1)**2+OEL(2)1C3<c2 + OEL(3)*;|c2) 
IF(TRVEL.GT.TTROEL) GOTO 120 
R00EL=SQRT(0EL(4)*:C2 + 0EL(5)**2+0EL(6)»*2) 
IF(ROOEL.GT.TROVEL) GUTO 120 
TERFLG=1 
C 

120 RETURN 
END 





^4g/-' 
'fvu :: 


h, 

y 
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c 

c 

C THIS SUBROUTINE CALCULnr&:b THt JOINT VELOCITIES REOUIKED TO 
C IMPLEMENT THE GIVEN END Ef^frcfOK* TRANSLATIONAL AND ROTATIONAL 

C VELOCITIES. 

C 

e rt.ft. PATTERSON 

C 

C 

SUBROUTINE SOLV£<THf I'HV t CTH » STH > JO INTS » END i VEL ) 

C 

REAL TH(S) rTHV(S) »CTH<7) ,STH(7) » JOINTS i 3 f 8 > p ENIK 3 M ) rVEL<6) . 
+ NOfSNf ESfHUf JLIM<8> » JVLIM(6) 

DIMENSION WRV€L<3) »ROTVEL(3) »RINVJ<3»3) 

C 

COMMON /ARMPAKVNOiSN.eSf HU 
COMMON /JNTPAKVJLIMf JVLIM 
C 

c 

C9~CTH(2)+CTH<7) 

S9=STH(2)+STH(7) 

C 

C* COMPUTE WRIST VELQCIJY. 

C 

URVEL ( 1 ) =VEL < 1 ) + < END < 2 > 3 ) < 6 ) -END < 3 » 3 ) *V£L ( 3 ) ) )kHU 

WRVEL ( 2 ) = VEL < 2 > + < END ( 3 » 3 ) *VEL ( A ) -END ( 1 f 3 > * VEL ( 6 ) ) *HU 
WRVEL(3)=VEL<3) + <END< 1 i3)#VEL(5)-END(2y3)*VEL<4) >5|cHW 
C 

C* COMPUTE INVERSE JACOBIAN FOR FIRST THREE JOINTS. 

C 

S3ES=STH<3>*ES 

RINV J ( 1 f 1 ) =»-STH ( 1 ) *S3ES 

RINVJ<lf2)=CTH(l)*S3ES 

RINVJ<1»3)=0. 

R I N V J i2fi> = JOI NTS ( 1 > S ) *STH ( 7 ) 

RINV J ( 2 » 2 ) = JOINTS < 2 > 5 ) ^IcSTH ( 7 ) 
RINVJ<2f3)a(J0INTS(3r5)-J0INTS<3r4) )¥S9 

S1S9=STH(1)*S9 

ClS9=CTH<i)*S9 

S9ES=^S9*£S 

RlNVJ(3rI)=*SlS9*SN-ClS9fS9ES 
RlNVJ<3^2)=i-ClS9»SN-SlS9lcS9ES 
RINVJ<3f3)*-< JOINTSOf 5)-N0>*S9 
C 

C* COMPUTE FIRST THREE JOINT VELOCITIES. 

C 

DIV=S9ES4tS3ES 

THVa )=(RINVJ<i » 1 )*WRVEL( 1)+RINVJ(1 >2)»URVEL(2) )/DIV 
THV<2) = <RINVJ<2f 1 }*URVEL(1 >+RINVJ(2f 2))|tURVEL<2)+RINVJ<2f3># 

+ WRVEL(3))/niV 

THV(3)=<RINVJ(3f 1 )¥URVEL( l)+RlNVJ(3»2>4cWRVEL(2)+RINVJ(3f 3)* 

+ WRVEL(3) )/DIV 

C 
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C* COMPUTE ENH EFFECTOR ROTATIONAL VELOCIflES. 

C 

UEL(4)=UEL< 4)+STH( 1 ) » ( THU < 2 ) + THU < 3 ) ) 
UEL(S)=UEL(5)-C7H( 1 > * < THU ( 2 ) + THU ( 3 ) ) 
UEL(6)=UEL<6)-THU<1) 

ROTUELC 1 )=*CTH( 1 ) *CTH < 7 ) *UEL ( 4 ) +STH (1 > *CTH < 7 ) I^UEL ( 5 ) - 
+ STH(7)*UEL<6) 

R0TUEL<2)=-STH( 1 ) *UEL ( 4 ) +C TH < 1 > »UEL < 5 ) 

R0TUEL<3)*CTH( 1 ) TH ( 7 ) J^^UEL < 4 ) +STH ( 1 ) *STH ( 7 ) 3»:UEL < 5 ) + 


+ CTH(7)*UEL(6) 

C 

COMPUTE LAST THREE JOINT UELOCITIES. 

C 

IF(ABS(STH(5) ) .LT. 1 .OE-2) GOTO 10 

THU(6)=»(CTH<4):«R0TUEL( 1 )+STH(4)«R0TUEL(2) )/S rH<5) 
THU < 5 ) =“STH ( 4 ) «K0TUEL (1 ) +LTH ( 4 ) *R0 fUEL ( 2 ) 
THU<4)*R01UEL(3)-CTH<5)*THU(6) 

GOTO 20 
C 

C* COMPUTE LAST THREE JOINT UELOCITIES FOR S5=0. 

C 

10 THU<6)=R0TUEL<3) 

THU(5)«-SIH( 4))rK0TUEL( 1 )+CTH( 4)4 lR0TUEL<2) 
THU<4)=0, 


C 

C* TEST FOR EXCESSIUE JOINT UELOCITIES. 


C 

20 RED=1. 

IF(ABS(THU< 1 ) > .GT. JULIMC 1 ) ) 
IF(ABS<THU(2) ) .GT. JULIM<2) ) 
IF(ABS<THU<3) ) .Gf. JULIM<3) ) 
IF(ABS<THU(4) ) »G1 . JULIM<4) ) 
IF(ABS(THU<5) ) .Gf .JULIM <5 ) ) 
IF(ABS(THU(6) ) .GT. JULIMC6) ) 
IF(R£D.EQ.l*) GOTO 30 
THU(i)=REi:):^cTHU(l) 
THU<2)»RED*THU(2) 
THU<3)=»RED«rHU<3) 
THU(4)=RED*THU(4) 
THU(5)=RED»THU<5) 
THU<6)»REIi*THU(6) 

C 

30 RETURN 
END 


RED=AMIN1 (REDf JULIM( 1 ) /ABS( THU< 1) ) ) 
REU*AMlNl<REti» JULIM(2)/ABS(THU<2) ) ) 
RED=AMlNl(REHr JULIM(3)/A&S(THU(3) ) ) 
RED=AMINl(REruJULlM(4)/A&S<THU<4) ) ) 
KED=AMIN1 (REIU JULIM(5)/ABS(THU(5) ) ) 
RED=AMIN1 (REH» JULIM(6)/ABS<THU(6) ) > 
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